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ABSTRACT 

In  this  work,  the  control  laws  of  a  bank- to- turn  mis- 
sile using  and  optimal  estimator  in  the  terminal  guidance 
phase  were  designed,  and  the  effect  of  increasing  the  number 
of  measurement  sensors  in  the  missile  to  generate  more 
information  on  the  state  was  investigated.   In  the  design  of 
the  control  law  the  modern  optimal  control  theory  with  the  a 
quadratic  performance  index  was  used.   Implementation  of 
this  control  law  required  the  use  of  a  Kalman  filter  as  the 
optimal  estimator.   The  extended  Kalman  filter  algorithm  was 
utilized  in  the  present  study  since  the  measurement  states 
were  non-linear  functions  of  the  state  vectors.   In  order  to 
test  the  effects  of  the  implementation  of  the  increased  mea- 
surement sensors,  two-,  four-,  and  six-measurement  sensors 
were  assumed  to  be  implemented  in  the  optimal  estimator.   By 
computer  analysis,  the  designed  guidance  laws  were  evaluated 
and  the  effect  of  the  implementation  of  increased  measurement 
sensors  was  tested. 

The  results  of  the  simulation  revealed  that  the  designed 
guidance  law  was  successful  within  the  specified  scenarios, 
the  effect  of  the  implementation  of  increased  measurement 
sensors  for  the  estimator  was  favorable  only  in  that 
increased  measurement  sensors  generated  more  information 
about  the  state  vectors. 
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I.   INTRODUCTION 

The  bank-to-turn  missile  has  high  lift  acceleration  in 
a  direction  perpendicular  to  its  wings.   For  airbreathing 
missiles  which  are  required  for  large  stand-off  ranges  it 
also  offers  the  advantage  of  lower  inlet  angles  of  attack 
than  that  of  skid-to-turn  missiles. 

Although  the  control  laws  using  the  modern  guidance 
control  theory  are  more  complex  than  the  normally  used 
proportional   navigation  air-to-ground  method,  they  have 
great  potential  for  maneuvering  targets  in  air-to-air 
engagement  situations.   The  application  of  optimal  control 
and  estimation  theory  to  bank-to-turn  missile  configurations 
has  been  tried  in  order  to  obtain  increased  performance.   In 
the  application  of  the  optimal  control  theory  to  the  bank- 
to-turn  missile,  it  is  necessary  to  have  information  on  all 
states  or  an  estimate  of  the  state   variables  that  have  not 
been  measured.   Since  the  state  information  available  from 
the  typical  missile  sensors  is  limited,  it  is  necessary  to 
employ  an  estimator.   The  estimator  used  in  this  study  was 
a  first  order  extended  Kalman  filter. 

The  present  work  addressed  the  design  and  evaluation  of 
the  optimal  state  estimator  and  optimal  control  laws  for 
application  to  a  bank-to-turn  missile.   In  the  development 
of  this  work,  Chapter  2  will  describe  the  kinematics  of  the 
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missile  and  formulate  the  equations  of  the  motion.   All 
major  assumptions  are  listed.   The  optimal  controller  is 
described  in  Chapter  3.   The  simulation  results  of  the 
controller  on  three  scenarios  described  later  are  used  to 
check  the  suitability  of  the  optimal  controller.   The 
optimal  estimator  is  covered  in  Chapter  4,  the  formulation 
of  the  state  equation,  measurement  equations  and  the 
derivations  of  the  elements  of  Jacobean  matrix  are  covered. 
The  extended  Kalman  filter  algorithm  for  non-linear  system 
is  then  reviewed.   Before  the  final  computer  simulation  a 
discussion  is  given  on  the  nominal  parameters  needed  in 
initializing  the  Kalman  filter.   The  results  of  the  simula- 
tion of  estimators  with  two-;  four-,  and  six-measurement 
vectors  on  a  typical  scenario  are  then  presented.   In 
Chapter  5,  the  simulation  results  of  the  control  laws  imple- 
mented the  estimator  with  two-,  four-  and  six -measurement 
sensors  on  three  scenarios  is  described.   The  detail  analy- 
sis of  the  results  is  developed  in  order  to  investigate  the 
effect  of  the  key  variables  of  the  control  system  on  a  bank- 
to-turn  missile.   The  mean  miss  distance  determined  from  50 
Monti  Carlo  runs  as  a  performance  standard  was  used  in  the 
analysis  of  the  results  of  the  control  laws  as  a  function  of 
the  key  variables.   Finally,  the  conclusion  are  summarized 
in  the  last  Chapter. 
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II .   KINEMATICS  OF  A  BANK -TO -TURN  MISSILE 

A.   ASSUMPTIONS 

The  geometry  and  the  equations  of  motion  of  the  missile 
will  be  developed,  representing  the  positions  in  space, 
under  the  following  assumptions: 

1.  The  local  geographic  coordinate  system  will  be  used 
as  the  inertial  reference. 

2.  To  simplify  the  equation  of  motion  the  stability 
axes  are  adopted  for  the  missile  body. 

3.  The  missile  velocity  due  to  thrust  is  constant. 
There  is  no  missile  acceleration  in  velocity  due  to 
thrust . 

4.  Each  control  surface  or  surfaces  is  rigid. 

5.  Relative  to  the  body  axes,  each  control  system  has 
only  one  degree  of  freedom.   The  missile's  control 
acceleration  vector  acts  normal  to  the  velocity 
vector,  that  is,  the  dot  product  of  acceleration 
vector  and  velocity  vector  is  zero,  also  the 
acceleration  acts  through  the  missile  center  of 
gravity . 

6.  The  Euler  angles  t>      ,    9    and  <b      will  be  used  to 

°      rv    v 

describe  the  orientation  of  the  missile  with 

respect  to  inertial  space,  where  <$>        and   6r   are 

horizontal  and  vertical  flight  path  angles,  and   $ 

is  the  roll  or  bank  angle. 
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7.   The  first  order  lags  with  time  constants   r    and 

P 

x    to  a  roll  rate  command   P    and  a  normal 
o  c 

acceleration  command   a   will  be  considered  for 

c 

the  dynamic  response  of  the  missile. 

B.   MISSILE  CONTROL  METHOD 

Before  going  into  the  mathematical  detail  concerning 
the  motion  of  a  missile  in  space  as  a  result  of  a  guidance 
command,  it  is  helpful  to  review  with  the  control  law  for  a 
bank-to-turn  missile.   The  guidance  system  detects  whether 
the  missile  is  flying  too  much  or  too  high  to  the  left, 
right  or  vertical.   The  guidance  system  measures  these 
deviations  or  errors  and  sends  signals  to  the  control  system 
to  reduce  these  errors  to  aero.   The  task  of  the  control 
system  therefore  is  to  maneuver  the  missile  quickly  and 
efficiently  as  a  result  of  these  signals.   In  a  bank-to-turn 
missile  system,  the  guidance  angular  error  detector  produces 
two  signals   R   and   <j>   in  terms  of  polar  coordinate  expres- 
sion, showed  in  Figure  2.1.   The  same  signals  can  be 
expressed  in  another  way,  that  is,  in  cartesian  coordinate. 
The  usual  method  is  to  regard  the  <p      signal  as  a  command 
to  roll  through  an  angle   a   measured  from  the  vertical 
and  then  to  maneuver  outwards  by  means  of  the  missile's 
elevators.   The  method  of  maneuvering  the  missile  is  as 
follows.   The   $   command  goes  as  a  positive  command  to  one 
control  surface  and  a  negative  command  to  the  other,  this 
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MISSILE 


TARGET 


FIGURE  2.1.   MISSILE  CONTROL  SYSTEM  IN  POLAR  CO-ORD. 

causes  the  missile  to  roll.   The   R   command  goes  to 
surfaces  always  as  a  positive  demand,  this  causes  the 
missile  to  accelerate  normal  to  the  velocity  vector.   The 
intention  is  to  make  the  response  in  roll  fast  so  that  the 
commands  can  be  applied  simultaneously  which  makes  for 
simplicity,  only  a  pair  of  control  surfaces  are  used  as 
ailerons  and  elevators  at  the  same  time,  control  is  obtained 
by  means  of  the  separated  servos. 

C.   GEO.METRY  OF  MOTION  IN  SPACE 

The  motion  of  a  missile  as  a  particle  may  be  described 
by  using  coordinate  measured  with  moving  axes  (relative- 
motion  analysis).   The  analysis  of  motion  can  be  simplified 
by  using  measurements  made  with  respect  to  a  moving  coordi- 
nate system.   In  present  work,  the  equations  of  motion  will 
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7        MISSILE 
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AXIS 


J      MISSILE  WING   AXIS 


FIGURE    2.2       GEOMETRY    OF    RELATIVE    MOTION 


be  described  with  respect  to  the  initial  missile  body  axes 

taken  as  the  inertial  axes.   After  launching,  the  missile 

body  axes  changes  position,  but  the  relative  position, 

velocity  and  acceleration  are  still  computed  in  inertial 

axes,  that  is,  in  the  initial  body  axes.   The  current  zero 

effort  miss  distance  is  calculated.   This  will  then  be 

transformed  to  values  in  missile  body  axes  to  compute  the 

amount  of  the  control  inputs  to  the  missile  using  Euler's 

transformation. 

Figure  2.2  shows  the  missile  and  target  as  points  with 

respective  vector  velocity   V   and   V\  .   In  this  analysis, 
^  m        t  ' 

because   IV  I   is  assumed  to  be  at  least   2 1 V . I  ,  and  the 

1  m '  '  t  !  ' 

angle  of  attack  is  assumed  to  be  small,  the  lead  angle   <j> 
between   Vn   and  the  missile  longitudinal  axis  is  small. 
Let  the  vector  denote  the  relative  position  of  the  target 
with  respect  to  the  missile.   The  orientation  of  the  sight 
line  vector  in  inertial  space  can  be  represented  by  the 
angle   yR   and   6R  ,  as  shown  in  Figure  2.2.   The  sight 
line  vector  is  resolved  into  three  components  in  inertial 
axes  as  follow  s  : 


Rx  =  R*cos(6R)  "ccs  (yR)  =  Xt  -  Xm 
Ry  =  R*cos(9R)  *sin(;jjR)  =  Yt  -  Ym 


Rz  =  -  R*sin  (cJ;R) 


t    -  Zm 


(2.1) 
(2.2) 
(2.3) 


Where  R   is  the  magnitude  of  the  sight  line  vector. 
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Define  the  relative  velocity  vector 


V   =  \\     -    V 

-  r    - 1    -  in 


•4) 


Where   V.   and   V   are  target  and  missile  velocities. 

The  rate  of  change  of  the  sight  line  vector's  magnitude 
is  equal  to  the  component  of   VR   along   R  .   It  can  be 
expressed  as  following: 


R  = 


R 


(2.5) 


V  *R   +  V  *R   +  V   *R 
rx   x ry   >• rz   : 

(R  2  +  R  2  +  R  2) 
v  x     v      z 


(2.6) 


Where   V   =  \*   -  V 

rx    tx    mx 


V   =  V+   -  V 
rv     ty    mv 


V  ,  =  V+   -  V 

rz     tz     m: 


are  the  relative  velocity  components  in  the  inertial  frame. 
In  the  geometry,  the  relative  angles   <Jjr   and   6R   also 
will  be  expressed  by  the  relative  vectors: 


R 


=  -  tan' 


R 

7 

2  2    YT 

x      > 


(2.6) 
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.  -1 

lb      =    -  t  an 


(2.7) 


IVh  ere 


and 


pn      are    the    elevation    and   the    azimuth    angles 


R   WWJ~   yR 

of  the  relative  sight  line  vector  in  the  inertial  frame 

Then  the  rate  of  change  of  both  angles  are 


V 


r9 


R 


R 


sine    cos;!/    V        -    sin9    sin^    V        +    cosG    V 
v   rx  v  v   ry 


v 


v   rz 


(R    ~    +    R    ~    +    R      )    '  ~ 
x  y  z    J 


(2.8) 


V 


TJ 


sin^ii   V 
_  v   rx 


v    rv 


Rcos^ 


(R   "    +    R 


y 


)  COS  0 


V 


(2-9) 


Where    Vnr       and      Vn ,       are    the    rates    of    change    in      6         and 

Re         R-p  a  v 

\p        components,  is  shown  in  Figure  2.3.   These  quantities 
will  be  used  later  in  developing  the  estimator  state 
equations  and  will  also  be  used  to  represent  the  variables 
for  the  missile  seeker.   The  quantities   6D  ,  GD  ,  ib      ,    ip      , 

K      K      K      K 

R   and   R   (corrupted  by  noise)  are  the  set  of  measurements 
assumed  available  from  the  missile  seeker. 


D.   KINEMATICS  OF  C0NTR01  VECTOR 

Figures  2.4  and  2.5  are  views  from  near  and  side  of 


miss  ile 


a   and   <b   represent  the  magnitudes  of  the 
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FIGURE    2.3      RELATIVE    VELOCITY    COMPONENTS    IN    POLAR   CO-ORD 
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-Z-AXIS 


MISSILE  NORMAL 
FORCE  AXIS 


a   cose 
m         •  v 


ecos  6 
to  v 


DEROLLED   Y-AXIS 


FIGURE    2.4       KINEMATICS    OF    CONTROL    VECTORS    VIEWED 
FROM    REAR   OF   MISSILE. 


Z-AXIS 


a   coscb 
m        Y 


HORIZONTAL 


FIGURE  2.5   KINEMATICS  OF  CONTROL  VECTORS  VIEWED 
FROM  SIDE  OF  MISSILE. 
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missile  acceleration  due  to  the  acceleration  command  (ac) 
and  the  bank  angle  due  to  the  roll  rate  command  (Pc)  each 
other.   g   represents  the  gravitational  effect  at  the  center 
of  gravity  of  the  missile.   As  mentioned  in  control  method, 
the  sign  of   a   is  such  that  it  is  positive  upward  with 
respect  to  the  wings.   Figure  2.5  shows  that  the  accelera- 
tion normal  to  the  velocity  vector  has  the  components  due 
to  the  elevator  control  input  and  the  effect  of  the  gravity. 
The  total  magnitude  of  the  acceleration  normal  to  the 
velocity  vector  can  be  represented  as 


a  -cos6 
m    Y 


-  ^*sin6 


The  velocity  and  acceleration  components  for  simple 
circular  motion  can  be  represented  as 


an  -  V-/P 


See  Reference  1. 

Using  this  definition,  the  differential  equations 
describing  the  rate  of  change  of  the  flight  path  angles  are 


a  cos >i  -  gcos6 
m    v        °  v 

Vm 


(2.10) 
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a      si  nil 
m 

V     c  o  s  3 

III  V 


(2.11) 


Note      amftcosu)    -    g*cosG         is    the   magnitude    of   the   normal 

°      v 

acceleration  component  to  the  velocity  vector  in  missile 
side  plane  and   am*sincj)   is  the  magnitude  of  the  normal 
acceleration  component  to  the  velocity  vector  in  the  x-y 
plane.   The  acceleration  component  of  the  missile  velocity 
V   has  only  the  component  of  the  effect  of  gravitv  in  the 
missile  side  plane  under  the  assumption  that  the  missile 
velocity  is  constant.   The  differential  equation  is: 


V. 


m 


g*sm6 
0     v 


(2.12) 


The  rates  of  change  of  the  missile  position  components 
(Xm,  Ym,  Zm)   can  be  obtained  by  integrating  the  components 
of  missile  velocity.   From  Figure  2.2,  the  components  of 
missile  velocity   (Ymx,  Ymy ,  Vmz)   in  inertial  frame  are 
given  by 

(2.15) 


X   =  v   =  V  *cos0  -cosUj 
m    mx    m     v     •  v 


Y      =    V        =    V  *cos8    *sini|j 
m  my  m  v  rv 


;   =  V 

m    m; 


V  *sin6 
m     v 


(2.14) 


(2.15) 


Let  the  response  of  the  missile  to  input  command  in 
normal  acceleration  and  roll  rate  be  considered.   If  a 
demand  is  made  on  a  missile  for  a  transverse  acceleration, 
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it  is  initiated  by  sending  a  signal  to  the  appropriate  con- 
trol surface.   The  missile  acceleration   a    follows  the 

demanded  acceleration   a,   in  a  manner  which  mav  be 

d 

characterized  by  a  frequency   u    (weather  cock  freouencvl 
and  a  damping  factor   P  .   For  simplicity,  in  present 
work,  only  the  first  order  lags  with  time  constant   t    and 

>        }  6  p 

t    are  assumed,  the  equation  of  the  first-order  system  can 
o  l 

be  represented  as  follows: 

X(t)  +  l./x*X(t))  =  f(t) 
Taking  the  Laplace  transformation  on  has 

X(S)  =  F(S)/(S+1./T) 

with  zero  initial  condition.   Applying  these  to  the  BTT 
missile  control  system. 


t  *a   +  a   =  a 

am    m    c 


t  *P  +  P  =  P 
P  c 


(2.16) 

(2.17) 


Rearranging  the  differential  equation 


a   =   (a   -  a  ) /t 
m      c    ma 


P  =  (P   -  P  )/*c 

v  c  .   m   p 


4)  =  P 


(2.18) 
(2.19) 
(2  .20) 


Where   a    and   P    are  the  acceleration  and  roll  rate 
c         c 

commands.   Equation  (2.20)  is  by  definition.   From  the 
above  differential  equations  the  control  ratios  are 
transformed  as  follows: 


a  (S)       1 
c       a 


£_  I  jJ_  -  _  ±_  (2.22) 


P  (S)    t  S  +  1 
c       p 


Equations  (2.10)  through  (2.22)  constitutes  the  dynamic 
equations  describing  missile  motion  in  response  to  the 
command  inputs   ac   and   Pc  .   The  solution  of  these 
equations  provide  missile  position   (Xm  ,  Ym  ,  Zm)  , 
orientation  and  magnitude  of  velocity  (ib      ,  9   ,  Ym)   and 
orientation  and  magnitude  of  control  acceleration   ( $  ,  am) 

E.   CO-ORDINATE  TRANSFORMATION 

Given  that  the  missile  is  on  guided  flight  to  compute 
the  magnitudes  of  the  control  inputs  at  any  moment,  it  is 
necessary  to  transform  the  instantaneous  values  from  the 
sensors  in  the  inertial  frame  to  the  instantaneous  values  in 
body  frame.  The  transformation  from  one  frame  to  another 
can  be  accomplished  through  a  transformation  matrix.  The 
transformation  matrix  wiill  be  developed. 

Define  an  inertial  coordinate  system  with  unit  vectors 
I,  J,  K.   Also, define  a  missile  body  axis  coordinate  system 
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with  unit  vectors  i,  j,  k.   It  is  desired  to  find  the  trans- 
formation between  the  I,  J,  K  system  and  the  i,  j,  k  system 

The  I,  J,  K  system  can  be  thought  to  be  oriented  so 
that  I  points  north,  J  east,  and  K  down.   Similarly,  the 
i,  j,  k  system  has  i  along  the  longitudinal  axis  of  the 
missile  (which  by  assumption  is  along  the  velocity  vector 
Vm) ,  j  out  the  right  wing,  and  k  down. 

Consider  three  successive  rotations  \b      ,  8    and  ±    . 

yv  '   v 

The  \p        rotation  is  about  the  inertial  z  axis,  and 

transforms  to  an  intermediate  axis  systems  i -,  ,  j -,  ,  k -,  . 

The   6    rotation  is  about  the  i,  axis  and  transforms  to  an 
v  J 1 

axis  system   i?,  j?,  k-,.   The  last  rotation  §      about  the 
i9  axis  transforms  from  flight  path  axes  to  missile  body 
axes.   As  a  result,  the  total  transformation  from  inertial 
to  body  axes  is  given  by 


[♦3Cev][*v] 


=  [a] 


i 
j 

K 


(2.23) 


Where  A  is  the  total  transformation  matrix  from  inertial  to 
body  frame.  The  elements  of  the  a  matrix  from  Reference  1 
are : 


all  =   cos(e  )*cos(ijj  ) 
^  v       v 

al2  =   cos  (  9  )  *sin(ilj  ) 
v       v 

al3  =  -sin(6  ) 
v 
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a21 

a:: 

a25 
a51 


ao 
a55 


7    = 


s  i  n  ( <j> )  *  s  in  ( 9 y 

s  i  n  ( o )       L  n  I  Ln  I 

s  i  n  (  i )  '••  c  o  s  ( 


cos(>)  -sin(  ;    j  :-co>  mi 

V  V 

cos(cp)  ssin(ev)  *sin( 
cos(i)  -cos ! 


cos(.<j>)  *sin( 


cos(i) *cos(  I . 

■ 


sin(  j>)  Asm(!)  J 
sin[  p)  -cos  :  T  ,.  I 
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J  1 1  .   AN  OPTIMAL  CONTROLLER 

Although  this  work  is  primarily  concerned  with  the 
effect  of  an  optimal  estimator  on  the  performance  of  a  bank' 
to-turn  missile,  we  will  first  outline  the  theory  of  an 
optimal  controller  in  this  section.   The  guidance  laws 
considered  here  were  first  developed  by  Stallard  [Ref.  10]. 
The  following  is  a  brief  outline  of  his  work. 

A.   GEOMETRY 

Figure  3.1  shows  the  applicable  geometry  representing 
the  Pro j ected-Zero- Control  miss  (PIC  miss)  distance  in  the 
terminal  state.   The  problem  is  considered  to  be  three- 
dimensional  and  two  major  axes  system  are  used: 

1.  A  seeker-oriented  axis  system  for  estimation  or 
measurement,  and 

2.  The  three  principal  axes  of  the  missile  for  the 
control  problem, 

Xb ,  Yb ,  Zb   denote  the  target  coordinates  relative  to  the 
missile  along  its  principal  axes.   The  body  system  is  repre 
sented  a  time   T  =  0   bv  a  set  of  Eulian  angles  which 
relate  it  to  an  earth  fixed  system.   The  angle   A<J)   is  the 
incremental  roll  angle  from  the  present  missile  axes  to  any 
future  orientation  at  time   T  ,  shown  dashed  in  Figure  3.1. 
The  PZC  miss  represents  the  total  Proj ected- Zero  effort  - 
miss  distance  and  is  defined  as  the  miss  distance  which 
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would  occur  if  no  further  control  was  exercised  on  the 

missile  at   T  =  Ti  .   It  is  composed  of  a  Y- components 
(PZCY  miss)  and  a  Z- component  (PZCZ  miss).   Under  the 
assumption  of  constant  velocity  the  X- component  (PZCX  miss) 
is  represented  by  the  distance  that  the  missile  lias  to  fly 
at  any  time,  since  it  is  not  effected  by  control,  it  is 
not  shown  in  Figure  3.1. 


Y,  at  time  t 


Yb  - 


♦I 


PZCZ 


a  A<J>  >h.i 


at  time   t 


PZC-miss 


FIGURE  3.1   COMPONENTS  OF  PZC,  MISS  DISTANCE. 
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B.   SUMMARIZED  DESCRIPTION  OF  AN  OPTIMAL  CONTROLLER 
Given  a  time- varying  linear  system  of  the  form, 


X  =  F(t)X  +  G(tJ  U 


(3.1) 


Where  X  is  n-component  state  vector 

U   is  m-component  control  vector. 
An  optimal  control  is  one  that  minimizes  a  performance 
index  made  up  of  a  quadratic  form  in  the  terminal  state 
plus  an  integral  form  of  quadratic  forms  in  the  control  and 
the  state: 


7  ^  _  ^  f  _  t  =  T  i  ^  +  5 


fT; 


T 


(XTAX  +  UTBU)dt      (3.2) 


Where  the   S,   and  A   are  positive  semidefinite  matrix  and 
B   is  a  positive  definite  matrix.   An  appropriate  choice  of 
these  matrices  must  be  made  to  obtain  acceptable  level  of 
X(Ti)  ,  (X(t)   and   U(t)  . 


The  guidance  law  determined  in  Reference  8  is  of  the 


form 


U 


(a,  ,  P, 

C       L. 


T 


With  the  performance  index  chosen  (the  weighting  on  the 
state  vector  was  not  considered  in  Reference  8. 
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Ti 


J  =  2    <Xb  +   V 


Jb  ^  I  Ti  +  2 


(U  -  Ub)T  B(U  -  Ub)dt 


(3.3) 


Where  the  first  term  is  one-half  the  required  miss  distance, 
the  second  term  is  included  in  order  to  eliminate  infinite 
energy  solutions.   B   is  a  2  by  2  positive  definite, 
symmetric  weighting  matrix  and  of  the  form 


1    0 


0 


(3.4) 


with   1/B. ■  =  (Ti  -  To)X   maximum  acceptable  value  of 

2 

|U.(T)j"   .   A  detailed  derivative  of  the  optimal  guidance 

law  is  given  in  Reference  10. 

The  solutions  of  the  above  equation  from  Reference  8 
give  the  optimal  acceleration  command  at  time   T   between 
TO   and   Ti   as 


5T 


so 


3B,  +  T 

1      20 


T  ( 


>W 


(3.5) 


Where   T     is  the  remaining  time  to  20  until  intercept  and 

ctq  00  r 

M,    is  the  PZC-miss  along  the  missile  normal  force  axis  and 
the  negative  si2n  is  due  to  definition  of  the  normal 
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acceleration  as  positive  in  the  upward  direction.   The 
optimal  roll  rate  command  at  time   T   between   TO   and   i'i 
from  Reference  8  is  given  bv 


21a  T 


(Mi  ) 
v  by' 


T    +  65  B0 
go        2 


(3.6) 


Where   M,    is  the  PAC-miss  along  the  wing  axis.   Defining 
by  o        o  o 

the  roll  error: 


a  j         -   - 1 |  bv 
A$  =  tan   I  tt^" 


(5.7) 


.An  alternate  form  for   P    can  be  obtained  bv  dividing 

c  ■         ° 

Equation  (5.6)  by  Equation  (3.5),  then 


/a 


T    (T 


_£° 


go 


+  5  B 


T     +  65  B0 


tan(A<j>) 


(5.8) 


go 


Because  the  solution  of   P   was  based  on  an  assumption  of 

c  t 

small  missile  roll  excusions,  the   tanA^  term  in 

Equation  (5.8)  can  be  replaced  by   A<J)   to  yield 


7a    T    T     +  5  B,) 
c    go  _go V_ 

a  2  T   5  +  65  B0 
c    go        2 


Acf> 


(5.9) 
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The  optimal  controller  is  implemented  in  the  present  work 
using  Equations  (3.5),  (3.7),  and  (5.9).   In  computing  th< 


control  input ,   T 


£0 


Y   and   Z -component  of  PZC-miss  and 


the  elements  of  the  weighting  matrix  are  needed. 


C.   APPROXIMATING  TIME-TO- GO 

It  is  necessary  for  the  Time-To-Go  and  the  Projected- 
Miss - Dis tance  in  missile  body  axes  to  be  specified  at  time 
T   to  compute  the  optimum  control  input.   The  Time-To-Go 
will  be  defined  at  first,  then  the  state  variables  will  be 
introduced  as  means  for  formulating  the  necessary  equations 
The  target  acceleration  components  of  the  state  variables 
will  be  modeled  since  it  is  assumed  that  any  information 
about  the  target  acceleration  cannot  be  obtained  from  the 
active  seeker  of  the  missile.   Then  using  this  target 
acceleration  model  the  algorithm  for  computing  the  other 
state  variables  will  be  developed. 

The  Time-To-Go  until  intercept,   T-  ,  is  required  for 
computing  the  control  input.   Let  the  time  to  intercept  be 
defined  as  the  time  to  minimum  range.   Under  the  assumption 
that  the  relative  velocity  vectors  may  be  considered 
constant  at  its  present  value  until  intercept.   T   can  be 
found  at  time,   T  ,  as : 


T.  = 

l 


T 

R    V 
— r  — r 


(3.10) 
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Where   R   is  the  present  position  of  the  target  relative  to 
the  missile  and  the  factor  in  brackets  in  the  component  of 
V   along   R  .   An  alternate  form  of  T   is: 


R  V    +  R  V    +  R  V 

T        — __  X     rX 


j_  9  ?  9 


V+V+V„  (3.11) 

rx     rv      rz  '  ^     J 


The  negative  sign  comes  from  the  sign  of   V    as  shown  in 
Figure  2.2  and  Equation  (2.6). 

D.   PROJECTED- ZERO- CONTROL  MISS  (PZC  MISS)  DISTANCE 

The  Proj ected- Zero-Control  miss  distance  (PZC-miss)  is 

defined  as  the  miss  distance  which  would  occur  if  there 

were  no  further  control  input  after  time   T  . 

In  this  work  the  state  vectors  are  defined  as  the 

following  state  variables 

X  =  (R  ,  R  ,  R  ,  V   ,  V   ,  V   ,  a.  ,  a^  t   sl.     )T 
—    v  x   y    z    rx   ry   r:    tx   ty   t  z J 


Where   R   is  the  relative  missile  distance  to  target 

r  ° 

V   is  the  relative  missile  velocity  to  target 
a    is  the  target  acceleration 
A  model  for  target  acceleration  has  been  assumed.   This 
is  given  as  follows: 


at(T)  =  atQe"  a'T|    a  >  0  (5.12) 
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Where   a   is  the  intial  target  acceleration  and  '■-       is   the 
reciprocal  of  the  acceleration  time  constant.   The  initial 
target  acceleration,   4g's,  will  be  assumed  for  each 
scenario,  and   a  =  1/20   for  evasive  maneuver  as  indicated 
in  Reference  10  will  be  chosen.   These  values  will  be  used 
in  the  simulation  of  the  optimal  controller. 

With  the  assumption  that  a  state  estimator  is  available 
to  provide  estimates  of  the  state  vector  at  current  time  (tj  , 
the  values  of  the  state  variables  for  no  control  inputs  at 
future  time,  can  be  obtained  easilv  using  the  integration, 
i.e.  , 


\ 


(t)  =  Vr(t0)  +  j*   at(T)dx 


t 

to 


(t)   =  R(t0)  +  f   V  fx)dT 

to  l 


The    results    of   these    integrations    are 


V   (t)    =    V    (tO)    +    l/o,    (1    -    e    "a(t        t0))at(t0)       (3.15) 


R(t)    =    R(t0)    +    (t    -    tO)*V    (to) 


+    [1/a    (a(t    -    tO)     -    1    +    e 


(5.14) 


■a(t    -    tO).  ,,       (tO) 


Where   tO   and   t   denote  current  time  and  future  time.   It 
should  be  noticed  that  above  integration  is  based  on  the 
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assumption  of  a  single  physical  dimension.  If  this  assump- 
tion is  to  be  applied  to  three-dimensional  problem,  it  must 
be  concluded  that  there  are  no  coupling  among  the  X,  V,  Z 

dimension.   Let   T    =  t  -  t   ,  then  the  components  of  the 

go        o  l 

PZC-miss  in  inertial  axes  at  current  time   (tO)   can  be 
expressed  as 


"  M 

X 

R 

X 

V 
rx 

M 

y 

M 

7 

Ti 

R 
y 

Rz 

+    T 
g° 

TO 

V 

rv 

V 

rz 

TO 


f 

0 

0 

X 

0 

f 

y 

0 

0 

0 

f 

tx 
tv 


tz 


TO 


(3.15) 


0 
0 
(l/2)*g*T 


oo  J 


"7  T1 

Where   f   =  f   =  f   =  l/a"[aT    -  1  +  e    go]   and  the  last 
x    y    z       L  go 

term  accounts  for  the  free  fall  effect  of  gravity  in  the 
vertical  axis . 

The  components  of  the  PZC-miss  in  missile  body  axes  for 
control  input  calculation  are  obtained  via  the  Euler  trans- 
formation matrix: 
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Kxl 

"by 

.Mb^. 

[*][e„I[*v] 


M 
) 
M 


"  M 

X 

M 
y 

M 

.     z  . 

(3.16) 


Where  it  is  assumed  that  the  missile  has  exact  knowledge  of 

its  orientation  angles   ijj   ,  8   ,  c|>  ,  from  the  accurate 

v    v 

inertial  measurement  units. 

The  values  of  optimal  control  vectors   (a    and   P  J  , 

r  v  C  C 

then,  are  calculated  from  time  to  go   (T   )  ,  PZC-miss  in 

°  CTO  ' 

o 

body  axes  and  weighting  matrix  components   (Bl  and  B2)  which 
may  be  determined  according  to  the  desired  miss  distance  and 
intercepting  time.   In  a  later  section,  the  weighting 
matrix  will  be  developed  and  the  scenarios  will  be  set  up 
for  computer  simulation. 

E.   SIMULATION  RESULTS 

The  guidance  law  in  this  work  was  tested  on  a  simple 
six-degree-of -  freedom  simulation  using  only  plausible 
numbers  without  reference  to  any  actual  missile  or  design. 

Before  the  computer  simulations,  the  weighting  matrix 
of  the  performance  index  selected  in  Reference  S  will  be 
calculated  using  the  initial  state,  X(0)  ,  the  missile 
operating  time  imposed,  T  (5  sec)  and  the  constraint  input 
vectors.   It  is  assumed  that  the  missile  has  a  velocity 
advantage  of  two  over  that  of  the  target,  a  zero-  and  0.5 
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second-time  lag  auto  pilots  for  pitch  with  limits  of  -3g's, 
+20g's  and  for  roll  rate  with  limits  of  +6.23  rad/sec. 

The  allowable  miss  distance  which  is  usually  determined 
by  sizing  of  the  warhead  was  chosen  as  5  meters.   Which 
makes  the  first  term  of  the  performance  index  Equation 
(3.3)  equal  to  12.5  meters.   The  acceleration  term  and  the 
roll -rate  term  in  the  performance  index  are  set  equal  to 
this  respectively  (one-half  of  square  mean  miss  distance), 
then 


>r 


(a   -  a,)  ~  dt  =  12.5  m' 
c     d 


(3.16) 


T. 


(P  ")  dt  =  12  .  5  m" 
~  c 


0 


(3.17) 


The  nominal  time  of  flight  for  the  terminal  phase  is  set 
equal  to  5  seconds.   The  components  of  the  weighting 
matrix  B  were  based  on  the  following  assumptions.   It  is 

9 

assumed  for  (4g's)  as  a  mean-square  value  of  (a   -  a,)   and 

2 
(2  rad/sec)   as  a  mean-square  roll  rate  to  be  acceptable. 

Then  the  components  of  weighting  matrix  are: 
B1  =  0.003254  sec° 
B   =  1.25  m"sec 
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The  commands   a   and   P   are  computed  every  0.05  sec 

and  are  frozen  when  the  Time-To-Go  falls  below  this  interval 

In  order  to  simulate  the  optimal  controller  appropriate 
scenarios  were  developed  below  with  restriction  that  the 
Time-To- Go  was  5  sec  and  the  speed  of  missile  is  two  times 
faster  than  that  of  target.   Three  scenarios;  tail -chase, 
head-on,  side- approach  engagements,  were  chosen  as  possible 
cases  in  this  work. 

In  case  1,  tail-chase  engagement,  the  target  flies  100m 
above  the  missile  and  2500m  uprange  with  a  horizontal  east- 
ward acceleration  of  4g's.   Figure  3.3  shows  the  case  2. 
In  case  5,  side- approach  engagement,  the  target  flies  400m 
above  the  missile  and  1900m  uprange  with  a  horizontal  north- 
ward acceleration  of  4g's  at   t  =  0   as  shown  in  Figure  3.4. 
For  simplicity,  when  the  scenarios  were  set  up,  1000m/ s  was 
taken  as  a  missile  speed,  500m/s  as  a  target  speed  in  case 
1  and  case  2,  600m/s  as  a  missile  speed,  300m/s  as  a  target 
speed  in  case  3,  and  4g's  as  a  target  acceleration  for  all 
three  cases.   When  simulation  was  executing,  this  first 
simulation  assumed  perfect  state  feedback  without  the  use  of 
an  optimal  estimator. 

The  simulation  results  are  shown  in  Figure  3.5  through 
Figure  5.10.   At  first,  the  optimal  controller  with  no  time 
lag  was  tested  for  three  scenarios,  as  shown  in  Figures  3.5 
and  3.6.   For  scenario  1,  the  normal  acceleration  command 
in  initial  phase  is  very  large,  then  decreases  to  zero  at 
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at  the  final  time.   For  scenario  2,  the  acceleration  is  z. 
in  the  initial  phase,  then  increases  and  converges  to  zero 
finally.   For  scenario  5,  the  acceleration  starts  from  zero, 
but,  increases  rapidly  up  to  the  maximum  acceleration  then 
freezes  for  seconds  before  decreasing.   The  acceleration 
does  not  converges  to  zero  at  the  final  time.   Comparing 
these  results  with  the  variation  of  the  roll  rate  command 
in  Figure  3.6,  the  opposite  trends  are  observed  in  the 
results  of  scenarios  1  and  2,  i.e.,  if  an  acceleration 
command  in  the  initial  phase  is  large,  a  roll  rate  command 
is  small.   The  acceleration  command  in  Equation  (5.5)  is  a 
linear  function  of  PIC-miss  in  of  z-direction  of  the  missile 
coordinate  system.   For  scenario  1,  PAC-miss  in  the 
z-direction  is  relatively  large,  this  causes  a  large  accel- 
eration command  and  a  small  roll  rate  command  in  initial 
phase.   For  scenario  2,  the  relative  magnitude  of  the  PZC- 
miss  in  the  z-direction  is  small.   This  cause  a  large  roll 
rate  command  in  the  intial  phase.   For  scenario  5,  the 
PZOmiss  in  y-direction  is  of  the  same  magnitude  as  the 
relative  distance  as  the  Time-To-Go  decreases.   This  means 
that  a  larger  acceleration  command  than  the  maximum  value 
allowable  in  the  missile  body  coordinate  system  is  required 
at  some  of  instance  time  during  the  missile  flying.   Thus 
the  acceleration  does  not  converge  to  zero  at  the  final  time 
In  the  derivation  of  the  equations  for  control  commands,  a 
small  angle  approximation  was  assumed.   However,  in  the 
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scenario  5  this  assumption  is  questionable.   The  roll  ra 
command  computed  as  shown  in  Figure  5.6  shows  a  sine  wave 
characteristics  which  is  possibly  due  to  overcorrection. 

The  simulation  results  for  a  0.5  second  tine  lage  are 
shown  in  Figure  5.8  and  Figure  5.9.   The  time  lag  causes 
much  larger  acceleration  control  commands.   This  includes 
overcorrections,  so  that  the  control  commands  do  not 
converge  in  the  final  phase.   The  other  characteristics 
follow  the  explanations  given  in  the  analysis  of  the  cases 
with  no  time  lag . 

For  all  three  scenarios  the  miss  distances  were  obtained 
in  the  simulation  of  both  cases,  no-  and  0.5  second-time 
lag.   The  miss  distances  are  below  0.5m  for  all  three 
scenarios  in  the  case  of  no-lag.   In  the  no  lag  case  the 
control  laws  gave  excellent  results.   In  the  lag  case,  the 
miss  distances  are  5.9m  for  tail-chase  engagement,  0.8m  for 
head-on  engagement,  and  52.9m  for  side-approach  engagement. 
The  miss  distance  of  the  side-approach  engagement  with  a 
different  maximum  acceleration  limit  fo  25g's  results  in  a 
miss  distance  of  less  than  5m.   This  higher  maximum  acceler- 
ation limit  will  be  used  in  the  scenario  5  during  the  simula- 
tions for  testing  the  performances  of  whole  control  system 
in  Chapter  5. 

The  previous  results  are  summarized  as  follows :  (1)  the 
tracking  of  a  target  is  very  dependent  of  the  missile  and 
target  geometry,  (2)  the  capabilities  of  the  missile 
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(maximum  acceleartion  and  maximum  roll  rate  limits  an J  time 
constant)  in  maneuvering  against  the  target  is  very  important 
factors,  (3)  higher  maximum  accelerations  are  required  in 
any  side  approach  engagement,  and  (4)  the  assumption  of 
small   AcJ>   in  the  optimal  control  law  is  a  limitation  in  the 
general  case.   Nevertheless,  the  control  laws  developed 
from  the  optimal  control  theory  for  both  the  no  lag  and  the 
0.5  second-time  lag  cases  yielded  successful  results  for 
the  specified  scenarios.   Since  the  optimal  controller  with 
0.5  second-time  lage  auto  pilots  resulted  in  a  miss  distance 
of  approximately  5m  with  complete  state  variables  feedback, 
it  is  expected  that  the  inclusion  of  an  optimal  estimator  for 
these  particular  scenarios  will  results  in  still  larger  miss 
dis  tances . 
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I V .   OPTIMAL  ESTIMATOR 

A.   DESCRIPTION 

Without  full  state  variable  feedback  it  is  necessary  to 
estimate  the  state  variables.   This  chapter  describes  the 
Kalman  filters  used  in  the  state  estimation.   These  filters 
were  designed  for  respectively:   (a)   two  measured  states, 
(b)  four  measured  states,  and  (c)  six  measured  states. 

All  measurements  have  noise  superimposed  on  them.   It  is 
assumed  that  the  nature  of  the  noise  source  is  known 
completely . 

The  Kalman  filter  is  the  optimal  estimator  of  the 
states.   Since  the  measurements  are  non-linear,  it  was 
necessary  to  use  the  extended  Kalman  filter  theory.   A 
first  order  extended  filter  was  assumed  in  this  work.   In 
developing  the  filter  algorithm,  the  non-linear  measurement 
equation  is  linearized  about  the  most  recent  state  estimate 
and  then  the  Kalman  filter  algorithm  is  applied.   This  is  a 
extended  Kalman  filter  algorithm.   The  results  of  the 
extended  Kalman  filter  will  yield  suboptimal  state  estima- 
tion . 

In  practice,  the  missile  will  have  sensors  of  various 
types  that  provide  inputs  which  are  related  in  some  fashion 
to  the  states  to  be  estimated.   The  main  emphasis  in  the 
present  work  will  be  on  active  seeker  which  is  assumed  to 
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provide  the  optimal  estimator  with  measurements  of  I     of- 
sight  angles  for  the  case  of  the  missile  having  two  measure- 
ment  sensors.   For  the  case  of  four  measurements  one  has  two 
of  line-of -sight  angles,  range  and  time  rate  change  of 
range.   For  the  case  of  six -measurements  one  has  two  of 
line- of -  sight  angles,  two  of  time  rate  change  of  the  line- 
of -sight  angles,  range  and  its  time  rate  change.   When  the 
measurement  equations  and  the  Jacobean  matrix  are  developed 
the  six -measurement  case  will  be  derived  since  the  other 
cases  have  the  same  kind  but  less  number  of  measurement 
s  ensors . 

We  will  first  consider  the  state  equations  and  then  the 
measurement  equations.   This  will  be  followed  by  a  discus- 
sion of  the  extended  Kalman  filter.   Measurement  error 
effects  on  the  initialization  of  the  Kalman  filter  will  be 
analyzed.   Finally,  the  estimator  for  the  measurement  cases 
will  be  simulated  in  order  to  test  the  effects  of  the  imple- 
mentation of  the  different  measurment  vectors  on  the 
extended  Kalman  filter. 

B.   STATE  EQUATIONS 

The  state  vector  is  limited  for  convenience  to  the 
following  state  variables: 

T 
X  =  (R   ,  R   ,  R   ,  V   ,  v   ,  V   ,  a.  ,  a.  .  a.  J 

-    K   x    '      y  '  z  rx'   ry    rz '   tx '   ty'   tz; 

(4.1) 


55 


Where      X,    =    R    ,    X_    =    R    ,    X_    =    R        are   the    components 

1      X     Z      y     3      Z  r 

relative  position,   X,  =  V   ,  X^  =  V   ,  X .  =  V      lie  com 
L  4    rx*   5     ry   0     rz 

ponents  of  relative  velocity,  and   X7  =  a«.v,  X„ 


X9  =  atz 


tx'  "8    "ty 

the  components  of  target  acceleration. 


In  order  to  develop  the  state  equations,  dynamic  equa- 
tions of  target  motion  are  needed.   The  target  model 
selected  for  tracking  applications  must  be  sufficiently 
simple  to  permit  ready  implementation  in  weapons  system  for 
which  computation  time  is  at  a  premium  yet  sufficiently 
sophisticated  to  provide  satisfactory  tracking  accuracy. 
To  meet  these  requirements,  the  model  described  by  Singer 
[Ref.  10]  will  be  used  in  this  work.   The  model  will  be 
presented  for  a  single  spatial  dimension  in  order  to 
enable  accurate  tracking  performance  estimates  to  be  made 
for  a  variety  of  sensor  measurement.   If  the  targets  under 
consideration  normally  move  at  constant  velocity,  the 
accelerations  due  to  turns  or  evasive  maneuvers  may  be 
viewed  as  perturbations  upon  the  constant  velocity  trajec- 
tory.  The  target  acceleration   a(t)  therefore  will  be 
termed  the  target  maneuver  variable,  in  a  single  physical 
dimension.   The  target  maneuver  capability  can  be  satis- 
factorily specified  by  two  quantities:  the  variance  or 
magnitude  of  the  target  maneuver  and  the  time  constant, 
or  duration,  of  the  target  maneuver.   Hence  the  target 
acceleration,  namely,  the  target  maneuver  is  correlated 


56 


in  time.  A  typical  representative  model  of  the  target 
acceleration  as  a  correlation  function,  can  be  express 
as  follows : 


l  (t)  =  E[at(t)at(t  +  t)]  =  a 


■a  t 


m 


a  >  0 


(4.2) 


Where   the 


m 


is  the  variance  of  the  target  acceleration 


and  a      is  the  reciprocal  of  the  maneuver  time  constant. 

2 

The  variance  a    ~      of  the  model  will  be  approximated  using 

the  following  formula  which  represents  the  acceleration 
probability  density  suggested  by  Singer  [Ref.  10]: 


max 


m 


[1  +  4P     -  Pn] 

L       max     0 J 


(4.5) 


where   a     is  a  maximum  acceleration  rate,   P      is  the 
max  '    max 

probability  of  maneuvering  at   a     ,  and   Pn   is  the 
v  '  &      max  '        0 

probability  of  no  target  maneuvering.   And  as  mention  in 
previous  chapter,   a  =  1/20   for  an  evasive  maneuver  will 
be  used  in  this  work. 

The  above  process  can  be  modeled  as  the  output  of  a  low 
pass  filter  driven  by  white  noise  and  it  can  be  described 
by  the  equation 


at  =  -  at  +  W 


(4.4) 


57 


where      a      (t)     ,    the   correlation    function    of   the   white   no 


input    sat  is  1 1 es 


•    2(t)    =    2ao    25(x) 
w    ^    J  m      v    J 


14.5) 


The    acceleration   model    above   will    be    applied   to    each 
X,    Y,    Z    dimension   with    no    cross    coupling,    the    system    state 
equations    can   be   written    then    as: 

X=FX+a   +   W 
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(4.7) 

The   Equation    (4.7)    is    a    linear    state    equation   with    the 
assumption    that    the    forcing    function    a    vector,    i.e.,    missile 
acceleration,    can   be    precisely   measured    and    also    resolved 
into      X,    Y,    Z    components    in    the    inertial    frame. 
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Many  sensors  have  a  constant  data  rate,  sampling  t 
data  every   T   seconds.   If  the  above  system  equation  is 

represented  in  discrete  equations.   The  appropriate  system 

equations  of  motion  may  be  expressed  by 


X(K  +  1)  =  :(T)X(K)  +  B(K)  +  u(K) 


14.8) 


where   v(T)   is  the  target  state  transition  matrix,   B  (K) 
the  deterministic  forcing  input  vector  which  is  composed  of 
first  and  second  integration  and   U(K)   the  inhomogeneous 
driving  input  due  to  white  noise.   Since  it  is  assumed  that 
the  missile  acceleration  is  a  known  deterministic  forcing 
function,  the  system  equation  of  motion  can  be  obtained  by 
direct  integration  in  each  single  dimension  with   E|W(t)|=0 
The  desired  integration  in  discrete  form  yields 


R(tO+T) 


V  (tO+T) 
r^ 


aT(tO+T) 


1  T   l/ot2(-l  +  aT  +  e"aT)   R(tO) 


0  1 


0  0 


l/a(l-e"aT) 


■aT 


-// 


tO+T 

tOam  (t^dtdt 


c   tO+T 

J   a„  (t)dt 


to 


m 


V  (tO) 
r 


at(tO) 


X(K  +  1)  =  *     (t  ,  a    )    X(K)  +   a  (K)    (4-9) 

—        m 

Where   K  +  1  =  (K  +  1)T  =  t  +  T,  K  =  KT  =  tO  . 
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Next,  if  the  driving  input   U('t)   vector  is  considered, 
this  input  is  not  a  sampled  version  of  the  continuous  tii 
white  noise  input   W(t)  •   Since   W(t)   is  white  noise, 
U(k)   also  should  be  a  discrete  time  white  noise  sequence 
that  is,   E[U(K)U(K+i-)]  =  0   for   i  +    0  .   Then  as  shown 
in  Reference  10,   Li   and   W   are  related  by 


u(k: 


,  CK+1) 

J       $ 

KT 


[K  +  l)t  -  t|   W(t)dt 


(4.10) 


Where   U(K)   is  a  white  noise  sequence  with  covariance 
matrix 


E[U(K)U(K) 


T 


Q 


(4.11) 


Following  the  above  derivation,  it  can  be  easily 

expanded  to  the  case  of  three  independent  dimensions  in   X  , 

Y,  Z.   For  a   =  a   =  a   =  a   ,and  a      =   a      =  a   =  a 
'  x    y    z  x    y    z 

since  the  model  of  target  acceleration  is  for  one  dimension, 
the  transition  matrix  is 


TI    f1I 


I    £9I 


0     0 


(4.12) 
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Where      I      and      0      are    5    by    3    identify    and   null    matri 


f,    =    1/a    (aT    -    1   .+    e 


f\    =    l/a(l    -    e"aT) 


-  ■:  r 


f-   =    e 


-aT 


The  forcing  acceleration  vector  is 
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(4.15) 
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The  covariance  matrix  of  white  noise   U(K)'  is 


Q 


Qnl    Q12l    Q13I 


Q12I     Q22I     Q25I 


Qi-i   Q?-i   Q«i 


15 


:o 


(4.14) 


Where 


Qn  =  (a2/a4)  [1  -  e"2aT  +  2aT  +  (2/3)  (aT)  3  -  2  (aT) 


-  4aTe"aT] 


-aT 


Q12  =  (o7a  )  [e  "-li  +  1  -  2e  ai  +  2aTe  a   -  2aT  +  (aT)"] 


2   2         -2aT         -raT 
Q13  =  (a  /a  ) [1  "  e  "al  -  2aTe  al] 


Q22  =  (a/a") [4e 


2,2,,,  -aT         -2aT 

+  2 


j  -  e  . 


aT] 


Q25    =    (a2/a)  [e"2aT    +    1    -    2e"aT] 


Q-    =    a2[l    -    e-2aT] 


C.   MEASUREMENT  EQUATION 

In  the  case  of  the  estimator  with  six -measurement  sen 
sors  the  present  study  assumes  that  the  active  seekers  of 
missile  give  the  measurements  of  the  sightline  angles 
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GR  ,  i|>R  ,  the  time  rate  change  of  the  sight  line  angles 
8 ,,  ,  '<Jn  ,  the  relative  ranae   R  ,  and  the  time  rate  change 
of  the  relative  range   R   contaminated  by  the  Gaussian  white 
noise.   Then  the  equations  of  measurements  in  discrete  time 
form  are 


Z(K)  =  h(x,K)  +  Y(K) 


I  1.15) 


h(x) 


where   Z_(K)   is  a  sequence  of  measurement  vector  contaminated 
by  white  noise   V(K)   and 

elevation  angle 
elevation  angle  rate 
azimuth  angle 
azimuth  angle  rate 
range 
range  rate 

The  missile  seekers  actually  measure  the  data  about  the 
seeker  axes.   Thus,  throughout  this  development  the  assump- 
tion is  made  that  the  missile  possesses  an  inertial  measure 
ment  unit  that  accurately  specifies  the  missile's  orienta- 
tion in  space  so  that  a  transformation  from  seeker  to 
inertial  coordinates  can  be  made. 

Referring  to  Figures  2.2  and  2.5,  the  six-measurement 
vectors  can  be  written 


3 
r 

q 

"  r 

— 

r  r 

R 

R 
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R 


tan 


R^ 

X  v 


V    .         r    R   V        +    R    R   V 

x   z    rx  v   z    ry 


2  ? 

(R        +    R      )V 

v    x  v         r: 


R 


(R+R+RUR+R 
v    x  v  z    ^  v    x  y 


2.1/2 


\b      =    tan 
+  r 


- 

■■ 

R 
y 

R 
X 

_ 

m 

ru 


r        R   cos 


R  V  +    R   V 
v    rx  x   ry 

(R   "  +    R      ) 

*•    x  y    J 


R=    (R2    +    R2    +    R") 
*•   x  y  z 


R 


(Vr.R)/R 


R   V        +    R   V        +    R   V 
x    rx y    ry z    r 

( R    2    +    R    2    +    R    2 ) l'Z 
K    x  y  z    J 


but      Rx    =    Xx    ,    Ry    -    X2     ,    Rz    =    X-     ,    Vrx    =    X4    ,    Vry    =    X.     , 


v      =  x, 

rz  t> 
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The   measurement    vector    can    be    expressed    in    terms    of   s 
vector 


h(x)     = 


hl 

1 

h2 

h- 

3 

= 

h4 

h5 

h6 

tan 


X, 

_____  T_T7 


X-Ax-X.    +    X92X_X,    +    [  fx-,  2    +    X   2)X.,X, 

1      j    4 2       _    _         '       1 2         j    b 

}  7  7     •?  /?  7  71/9 

'■xr  +  V  +  x3  >      <xi    T  X:"J 


tan 


X2 


1 


X1X2X4    +    X1X2X5 


t\     -  x2'   +  x3z)1/z    (X^   +   X,") 


?  ? 


(X  -   +  x/   *  x,-) 


2 ,  1/2 


xnx,  +  x0x_  +  x_x, 

14  2    5  _    6 


?  ? 


(xr  +  x,  +  x_-) 


1/2 


(4.16) 


The   measurement   noise    covariance    matrix    for    the    active 
seeker   can   be   written 


E[V(K)V(K)T]    =    R    = 


aQ         0  0  0  0  0 

0  a'2  0  0  0  0 

o 

0  0  a,  2  0  0  0 

0  0  0  a*2  0  0 


0  0  0  0 


aR2      0 


0  0  0  0  0a 

65 


? 


(4.17) 


R 


Where  the  diagonal  elements  are  the  variances  of  th   i  di- 
vidual measured  quantities. 

As  shown  in  Equation  (4.16),  the  measurement  vectors  are 
the  non-linear  functions  of  state  vectors.   It  is  impractical 
to  implement  in  non- linear  form  because  the  computation  of 
gain   K   and  error  covariance  matrix   P   in  update  process 
in  extended  Kalman  filter  algorithm  is  not  possible.   To 
simplify  this  computation,  and  to  implement  the  extended 
Kalman  filter,  the  Jacobean  or  matrix  of  partial  derivatives 
H   will  be  determined,  i.e., 

3h. 


H.  • 

1"! 


9X 


(4.18) 


The  Jacobean  will  be  a  6  by  9  matrix  because   h   is  a  vector 
with  six  elements  and   X   is  a  vector  with  nine  elements. 
Performing  the  operation,  the  components  of  the  matrix   H 
are 


H    =  (-X  X  )  /  (D    D   ) 

44        12     0  1 


=  (X  X  )/  IE  D   ) 

45  12     0  1 

=  0   H    =  0   H 

46  47         4  E 

=  (X  )/(D  ) 

51      1     C 

i    =  (X  )/(D  ) 

_  *.  2.  L 

I    =  (X  )/(D  ) 
53      3     C 


=  0   H    =0 
49 


66 


OH  =    0       rt  =    J       H 

5  5  5  6  57 


j        H 


bb 


3        V:     =     0 
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c  2 
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6  5 
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L       0       4  2     1   z1  J 
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5      3 
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D.   THE  EXTENDED  KALMA.N  FILTER 

Optimal  estimators  that  minimize  the  estimation  error, 
can  be  divided  into  two  processes,  the  one  is  the  updating 
process  to  estimate  the  state  vector  at  the  current  time, 
based  upon  all  past  measurements,  the  other  is  the  extra- 
polating process  to  estimate  the  state  at  some  future  time. 
Optimal  estimation  procedure  for  linear  state  equations  and 
non-linear  measurement  in  the  form  of  discrete  time  will  be 
described  referring  to  Reference  6.   Figure  4.1,  a  timing 
diagram  shows  the  flow  of  the  various  quantities  involved 
in  the  discrete  optimal  filter  equations. 

The  discrete  system  equation  whose  state  at  time   kt   is 
denoted  by  simply   X(K)  ,  where   U(K)   is  a  zero  mean, 
white  sequence  of  covariance   Q(K)  ,  is 

X(K)  =  $  X(K  -  1) ,  +  B(K  -  1)  +  U(K  -  1)     (4.21) 

where   0   is  a  transition  matrix 


E[U(K)] 


(4.22) 


E[U(i)U(j)T]  =  Q5i;j 


(4.23) 


The  measurements  are  the  non- linear  function  of  the  system 
state  variables,  corrupted  by  uncorrelated  white  noise  of 
covariance   R(K)  .   The  measurement  equation  is  written  as 
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where 


Z(.K)  =  h  (X,K)  +  V(K) 


E[U(K)]  =  0 


E[V(i)V(j)T]  =  Re 


ij 


(.4.24 

(4.25J 

(4.26) 


The  initial  conditions  are 


A 


A 


E[X(0)]  =  X(0)    ECCX(O)  -  Xn)(X(0)  -  Xn)]  =  P 


~QJ  Vii, 


-0 


0 


(4.27) 


E(WkVjT)  =  0 


uncorrelated 


(4.2S) 


The  Kalman  filter  algorithm  is  to  minimize  the  estima- 

A. 

tion  error,  i.e.,  error  covariance.   If   X   denotes  an  esti 

mate  of  state  vector,   X  ,  and   X   is  the  mean  value  of  the 
state  vector,  the  error  covariance  matrix  is  defined  as 


P  =  E[(X  -  X)(X  -  X)1] 


(symmetric)     (4.29) 


The  development  of  the  extended  Kalman  filter  algorithm  with 
a  few  definitions  is  essentially  more  application  of  the 
expectation  operator  E.   We  will  consider  the  updating 
process  then  the  extrapolating  process.   The  update  equa- 
tions are  used  to  incorprate  the  latest  measurement  in  the 
estimate  and  in  the  covariance.   After  we  obtain  the  updated 
covariance  matrix  we  will  then  consider  the  optimum  choice 
of  Kalman  gain  and  derive  the  equation  for  the  esti- 
mation of  state.   Let   X(K-1)(-)   denote  an  estimate  of   X 
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FIGURE  4.1   DISCRETE  EXTENDED  KALMAN  FILTER  TIMING  DIAGRAM 
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at  time   k-1  ,  given  measurements  up  to  and  includi  . 
K-2  ,  P(K-1)(-)   denote  the  error  covariance,   K(.K-l) 
denote  Kalman  gain  and   Hf.lc-1)  denote  the  Jaccobean  matrix 
at  time   K-1  .   We  will  seek  an  optimal  observer  in  the 
presence  of  the  state  excitation  noise  and  in  the  presence 
of  the  observation  noise. 


P(K-1)(+)    E{[I  -  K(K-1)H(K-1)]X(K-1)[X(K-1)T(-)  (I-K 


(K-1)H(K-1)T)+Y(K-1)TK(K-1)T] 


+  K(K-1)V(K-1)[X(K-1)  (-)T(I-K(K-1)H(K-1)T) 


+  V(K-1)TK(K-1)T]} 


4  .30) 


Rearranging  Equation  (4.50)  using  the  definitions 
Equations  (4.26)  and  (4.29)  and  the  assumption  that  the 
measurement  errors  are  uncorrel ated ,  then  one  has 


P(K-1)(+)  =  [I-K(K-1)H(K-1)]P(K-1) (-)[I-K(K-1)H(K-1)T] 


(4.31) 


+  K(K-1)R(K-1)K(K-1) 


T 


This  equation  updates  the  error  covariance  matrix. 
Then  we  still  need  an  optimal  gain   K.   We  will  choose  to 
minimize  the  diagonal  elements  of  the  covariance  matrix, 
i.e.  , 

J(K-l)  =  E[X(K-1)(+)TXV(K-1)(>)]  =  trace  [P(K-l)O)]    (4.32) 
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FIGURE  4.2   SYSTEM  MODEL  AND  DISCRETE  KALMAX  FILTER. 
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Introducing  a  bit  of  matrix  calculus,  one  has 


~    [trace  A  B  AT]  =  2AB 


4.32 


aK(K-l) 


[l-K(K-l)H(K-l)  J  =  -H(K-l) 


T 


(4.34) 


Applying  these  formulas  to  Equation  (4.31)  and  solving 
for   K(K-l)  , 


T 


r 


K(K-l)  -  P(K-l)  (-)H(K-l) A [HfK-l)P(K-l)  (-)H(K-l) l+R(K-l)  ]  ~  1 

(4.55) 
This  is  the  Kalman  Gain  Matrix. 
Rearranging  Equation  (4.31) 


P(K-1)C+)  =  [l-K(K-l)H(K-l)]?(X-l) (-1) 


(4.36) 


Then  the  update  state  estimate  at  time  K-l  is  equal  to 
the  extrapolated  state  estimate  at  time  K-2  plus  a  term 
which  weights  the  measurement  residual  via  gain   K(K-l)  . 

1((K-1)(  +  )  ='X(K-1)(-)  +  K(K-l)[Z(K-l)-h(k-l),/X(K-l)(-))] 

(4.37) 
We  need  to  consider  also  the  propagation  of  the  estimate 
X(+)   between  measurements  and  the  propagation  of  the 
covariance  matrix   P(K-1)(+)   between  measurements.   Propa- 
gation of  the  estimate  is  straight  forward  and  involves  only 
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the  application  of  the  state  transition  matrix  and  t] 
forcing  terms . 


X(K)C-)  =  I>(K-1)/X(K-1H+)  +  U(K-.l)      (4.33) 


iy  the  definition  (4.29),  solving  for   P(K)(-) 

P(K)(-)  =  <?(K-1)P(K-1)  (  +  )i(K-l)T  +  Q(K-l)    (4.59) 


Figure  4.2  illustrates  the  above  developed  equations  in 
block  diagram  form  which  contains  a  system  model,  measure- 
ment process  and  the  Kalman  filter  to  be  implemented,  and 
Figure  4.5  shows  a  simplified  computer  flow  diagram  of  the 
discrete  Kalman  filter. 

Equations  (4.55)  through  (4.59)  constitute  the  recursive 
formulas  for  implementing  the  extended  Kalman  filter 
algorithm.   The  process  is  initializing  by  providing  values 
x(0)(-)   and   p(0)(-)  .   In  order  for  the  extended  Kalman 
filter  to  be  simulated,  the  next  section  will  describe  the 
initialization  of  the  Kalman  filter. 

E.   INITIALIZATION  OF  THE  KALMAN  FILTER 

The  one  objective  of  this  work  is  to  test  the  perform- 
ance of  BTT  missile  control  system  having  a  Kalman  filter  as 
an  optimal  estimator  as  a  function  of  different  measurement 
vectors.   All  values  for  initialization  of  former  worker  in 
this  work  (Ref.  14)  will  be  used  for  computer  simulation. 
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1 •   I nit  iali zing  the  Error  Covari  ance  Matrix ,  P 

Let   a   ,  a    and   a    define  the  standard  deviati  n 
p     v        a 

of  relative  position  estimate,  of  relative  velocity  esti- 
mate, and  of  target  acceleration  estimate.   The  initial 
values  of  error  covari ance  matrix   p  ,  will  be  chosen  as 
follows.   The  first  six  elements  of  the  initial  state 
vector  are  determined  by 

UO)    =    X(0)  +  V(0) 

Where  the  measurement  vector  X  is  the  true  measurement 
vector  X  plus  V  caused  by  measurement  uncertainties. 
The  expected  mean  square  of  initial  covariance  matrix 
components.  With  the  assumption  that  the  initial 
covariance  matrix  is  diagonal  with  the  diagonal  elements 
reflecting  the  uncertainties  associated  with  the  initial 
state  vector  estimates,  the  components  of  the  covariance 
matrix  are 


E[V1  ]   =  E[V2Z]   =  E[V5-] 


E[V/J   =  E[V,  ]   =  E[V/] 


v 


The  last  three  elements  of  the  state  vector,  acceleration 
components,  are  set  to  zeros 


x7  =  x8  =  x9  -  0 


2.   Initializing  the  Measurement  Noise  Variance  Matrix  R 


Measurement  errors  tor  the  active  seeker 
assumed  to  be  due  to  thermal  noise,  gimbal  angle 
error,  environmental  noise,  and  glint.   The  values  of  one 
sigma  errors  in  each  measurement  will  be  used  in  present 
study.   Because  in  the  case  of  angle  measurement,  the  envi- 
ronmental noise  is  proportional  to  the  square  of  range  to 
target  and  glint  errors  the  wander  of  the  apparent  target 
centroid  as  seen  by  the  seeker  as  varing  inversely  with 
range  to  target.   The  errors  caused  bv  noise  are  generallv 
a  highly  complex  function  of  the  target  geometry,  target 
radar  cross  section,  radar  receiver,  signal-to-noise  ratio 
and  etc.   For  simplicity,  in  present  study,  the  constant  one 
sigma  errors  (average  values)  in  each  measured  vector  are 
assumed  to  initialize  the  Kalman  filter.   Referring  to 
Reference  15,  the  one  sigma  values  of  the  average  errors 
for  measurement  of  angle,  angle  rate,  range  and  range  rate 
of  the  present  active  seeker  are  tabulated  below. 


(1)  angle  measurement  errors 

(2)  angle  rate  measurement  errors 
(5)   range  measurement  errors 
(4)   range  rate  measurement  errors 

Before  the  simulation  runs  is  undertaken,  it  will  be 
briefly  discussed  the  manner  in  which  the  Kalman  filter  on 
board  the  missile  is  initialized  at  the  start  of  the 


0.15  -0 .6  deg 
0.5   -2.0  deg/sec 

5  meters 

6  m/sec 
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engagement  based  on  sensor  data  from  the  launch  aircra  :t . 
The  launch  aircraft's  sensors  are  assumed  capable  of  leter- 
mining ,  within  some  prescribed  accuracy,  the  relative 
position  and  relative  velocity  of  the  target  with  respect 
to  the  missile  at  the  instant  of  launch.   This  information 
is  sued  to  initialize  the  first  six  elements  of  the  filter's 
state  vector.   However,  the  launch  aircraft  is  assumed  to 
be  able  to  provide  no  information  regarding  target 
acceleration.   Thus  the  three  elements  of  the  initial  state 
vector  are  set  to  zero,  finally,  the  complete  list  of 
nominal  parameters  for  initialization  is  provided  in  Table 
4.1. 

F.   EVALUATION  OF  THE  OPTIMAL  ESTIMATOR  PERFORMANCE 

The  performance  of  the  state  estimator  with  two- 
measurement  vectors  (case  1) ,  with  four-measurement  vectors 
(case  2) ,  and  with  six-measurement  vectors  (case  5)  ,  was 
simulated  to  evaluate  the  effect  of  the  possible  implementa- 
tion of  the  more  measurement  sensors  on  the  bank-to-turn 
missile  for  typical  scenario  (tail  chase  engagement  case). 
The  error  covariance  and  the  Kalman  gain  components 
selected,  and  the  states  estimated,  were  computed  as  shown 
in  Figure  4.4  through  Figure  4.30.   The  control  laws  imple- 
mented with  the  estimator  also  were  tested  to  evaluate  the 
performance  of  the  control  system  of  the  BTT  missile  as 
shown  in  Figures  4.51  through  4.55. 
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Figures  4.4  to  4.12  represent  the  variations  of  t 

error  covariances  of  each  state  variable.   Figures  4. 

through  4.7  show  the  three  covariances  for  the  relative 

eositions.  i.e.,   P,n  ,  P~0  ,  P,,  .   Only  the  covariance 
'  1122     33 

of  the  two  measurement  case  varies  substantially.   Thus 

one  would  expect  that  the  gain  components   G-, -,  ,  G?,  ,  G--. 

would  show  only  variations  in  the  two  measurement  case  as 

are  shown  in  Figures  4.15  throueh  4.15.   The  trend  noted  in 

the  set  of  graphs  (4.4,  4.5,  4.5)  and  in  the  corresponding 

Kalman  gains  is  also  observed  in  the  other  covariance 

components.   Thus  the  covariance  components  for  the  velocity 

(Figures  4.7,  4.S  and  4.9)  should  larger  variation  is 

reflecting  in  the  corresponding  Kalman  gains  of  4-16,  4-17, 

4-18.   The  covariances  of  the  acceleration  components  show 

a  similar  development  but  there  appears  to  be  less  variation 

in   a.    and   a„_    acceleration  covariances.   In  the  case  of 
ty        tz 

at    the  variation  of  the  Kalman  oains  are  restricted  to  a 
tz  ° 

much  narrower  range.   The  error  covariance  and  the  Kalman 

gain  components  in  all  three  cases  eventually  converge  to 

very  small  values.   Figure  4.22  through  Figure  4.30  show  the 

results  of  the  state  estimations  of  the  three  estimators. 

Figures  4.22,  4.25  and  4.24  show  the  variations  of  the 

estimated  relative  position  (R  ,  R  ,  R  ).   Figures  4.25, 

4.26  and  4.27,  the  variations  of  the  estimated  relative 

velocity  (V  ,,  V   ,  V   ).   Figures  4.28,  4.29  and  4.50  the 
J         rx '   rv    j-z 


variations  of  the  estimated  target  acceleration   (a.  , 
a   ,  a   )  .   The  trends  of  the  curves  representing  the 
estimated  states  are  similar  except  for  Figure  4.25  which 
shows  a  wide  variation  in  the  estimated   V     component  of 

i   A. 

the  twro  measurement  case  from  the  true  state  values. 
Generally,  the  estimated  states  of  the  estimator  with 
two-  and  four-measurement  are  close  to  the  true  states, 
in  spite  of  the  wide  variation  of  error  covariance  compo- 
nents of  two-measurement  case.   The  estimator  with  six- 
measurement  vectors  generates  the  estimated  states  which 
show  almost  same  characteristics  as  the  estimator  with 
four-measurement  vectors,  but  usually  underestimated  the 
states  in  comparison  with  the  generated  states  of  the 
estimator  with  four-measurement  vectors.   Figure  4.32  and 
Figure  4.54  show  the  curves  of  the  control  commands  com- 
puted using  the  estimated  states.   The  results  of  the  six- 
measurement  case  have  a  wholly  different  form  from  the 
curves  representing  the  computed  control  commands  in  the 
other  cases.   The  normal  acceleration  commands  computed 
using  the  estimted  states  with  the  s ix  -measurement  vectors 
initially  have  smaller  values,  but  finally  reach  maximum 
value  as  shown  in  Figure  4.52.   Also  as  shown  in  Figure 
4.54,  the  roll  rate  commands  run  from  the  minimum  limit  to 
the  maximum  limit.   The  large  variations  of  the  both  control 
commands  of  case  5  during  a  short  time  interval  result  in 
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the  worst  miss  distance,  15.4m.   The  control  commands      he 
other  cases  behave  almost  like  characteristics  repres 
that  the  control  commands  are  increasing  smoothly  :   > 
zero  to  its  maximum  control  command,  after  two  or  three 
seconds  then  slowly  decreasing  to  converging  values.   The 
relatively  small  variations  of  the  both  control  commands 
during  a  short  time  interval  comparing  with  the  control 
commands  of  case  5  result  in  9.7,  miss  distance  in  case  1 
and  8.7m,  miss  distance  in  case  2.   From  above  results,  the 
estimator  with  four-measurement  vectors  gives  the  best 
result,  the  estimator  with  six-measurement  vectors  the 
worst  result  and  the  estimator  with  two-measurement  vectors 
almost  same  result  as  the  of  four-measurement  case  comparing 
with  the  result  of  the  estimator  with  six-measurement  vectors 

In  original  measurement  equation,  for  case  1,  the 
missile  has  two-measurement  vectors  which  give  the  informa- 
tion about  the  relative  angles,   6  n   and  \b  n  ,  was  assumed. 

&    '    R         R 

These  two -measurement  vectors  are  non- linear  functions  of 

three  relative  distance  components,  X-,,  X_  ,  X_  .   For  case  2, 

the  missile  has  four-measurement  vectors  which  give  the 

information  about  above  two  angles  plus  relative  range  and 

time  rate  change  of  relative  range  which  are  also  non- linear 

functions  of  the  components  of  six  state  vectors,  X-,  ,  X-,, 

X, ,  X,,  Xr   and   X,  .   For  case  3,  eventhough  the  missile 
3    4    5         6 

has  six-measurement  sensors  are  assumed,  the  measured 
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vectors  using  the  six-measurement  sensors  contain 
state  vectors  which  are  exactly  same  kinds  and  n  u 
the  measurable  state  vectors  using  the  four  measure 
sensors  in  case  2.   As  the  angle  rates,   5   }  ^   5 
included  to  the  estimator  as  the  measurement  vectors  in 
case  5,   the  both  angle  rates  are  non-linear  functions  of 
only  six-state  vectors,   X.,  X  ,  X-.,  X  ,  X-   and   X   . 
The  results  of  the  estimator  with  four-measurement  vectors 
and  with  six-measurement  vectors  show  the  almost  same 
variations  of  the  error  covariance  and  the  Kalman  gain  com- 
ponents  of  the  six  measurement  case  as  of  the  four  measure- 
ment case  as  shown  in  Figures  4.4  through  4.21. 

The  Kalman  filter  in  this  work  is  a  first  order  filter. 
Thus  the  components  of  the   Jacobean  matrix  contain  only  the 
first  order  term,  i.e.,  all  higher  order  terms  are  neglected 
As  the  order  of  the  system  is  increased  one  would  expect  a 
more  complex  system  to  response  differently.   The  above 
reasons  affect  to  the  underestimation  of  the  states  of  six 
measurement  case  as  shown  in  Figures  4.22  through  4.30. 
From  the  above  considerations  it  can  be  concluded  that  as 
the  increased  number  of  measurement  vectors  is  implemented 
to  the  extended  Kalman  filter  algorithm,  the  result  of  the 
system  may  not  be  enhanced  up  to  the  expected  degree  due  to 
the  increased  complexity  of  the  system.   The  system  with  the 
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first  order  filter  as  a  optimal  estimator  may  con 

to  make  less  desirable  results  due  ro  the  effec    /  . 

lectins  of  the  higher  order  terms  in  the  filter  alsoritl 
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v-   PERFORMANCE  H VALUATION  OF  THE  CONTROL     'EM 

This  section  will  discuss  the  performance  of  t  le  control 
system  implemented  in  a  bank-to-turn  missile  for  each  scen- 
ario.  The  maximum  normal  acceleration,  the  maximum  roll 
rate  and  the  time  constant  are  the  key  parameters  which 
reflect  the  missile  capabilities,  the  noises  to  measured 
vectors  are  the  parameters  of  the  environmental  effect,  and 
auxiliary  variables  of  some  importance  are  then  total  engage- 
ment time  and  missile  velocity.   The  mean  miss  distance 
determined  from  the  50  Monti  Carlo  runs  was  used  as  a 
performance  standard  for  the  estimators  of  tuo-  (case  1)  , 
four-  (case  2)  and  six-measurement  vectors  (case  3)  on  each 
scenario.   The  simulation  results  are  shown  in  Figures  5.1 
through  5.19.   The  sensitivities  to  variations  of  the  para- 
meters will  be  discussed  below.   Since  the  simulation  results 
show  essentially  the  same  characteristics  for  the  tail -chase 
engagement  (scenario  1)  and  for  the  head-on  engagement 
(scenario  2).   The  performance  on  these  two  scenarios  will 
be  discussed  together  for  each  parameter.   The  side- 
approach  engagement  (scenario  5)  will  be  discussed  sepa- 
rately. 

Figures  5.1  and  5.2  show  the  miss  distance  variation  as 
a  function  of  missile  maximum  normal  acceleration.   For 
scenario  1,  case  1  shows  a  mean  miss  distance  of  9 . Sm  at 
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above  log's,  case  2.9m  at  above  15g's  and  case  5,  L5. 

above  15g's.   Each  case  also  shows  that  as  ti    axi       tit 

of  the  normal  acceleration  decreases,  the  mean    ; : 

in  case  1  is  Jess  sensitive  than  in  the  other  two  cases. 

For  scenario  2,  the  result  of  case  1  shows  the  minimum  mean 

miss  distance  is  10m  and  the  result  of  case  2  is  17m  at 

a     =  21crrs  respectively,  the  result  of  case  3  is  52m  at 

max     a      v  '  ' 

a     =  17g's.   The  trends  of  the  curves  show  that  the  mean 
m  ax     ^ 

miss  distances  in  case  1  and  case  2  vary  in  same  manner, 

which  rises  sharply  as  the  maximum  acceleration  decreases. 

On  the  other  hand,  the  mean  miss  distances  in  case  5,  vary 

smoothlv  above   a     =  13g's  .   One  may  conclude  from  the 

max     ° 

analysis  as  follows:  for  scenario  1,  the  mean  miss  distance 

can  be  decreased  since  more  information  on  the  state  vectors 

can  be  obtained  from  the  increased  measurement  vectors  in 

estimator  for  low  maximum  acceleration.   In  order  to  get  the 

mean  miss  distance  below  20m  for  all  three  cases,  one  needs 

a    of  over  llg's.   For  scenario  2,  as  the  measurement  state 
max  °  ' 

vectors  are  increased  in  estimator,  the  mean  miss  distance 
does  not  decrease,  possibly  due  to  the  effect  of  neglecting 
the  higher  order  terms  in  the  extended  filter.   As  the  mag- 
nitude of  the  values  of  the  missile  and  target  geometry 
components  becomes  larger,  the  effect  of  the  linearization 

becomes  more  pronounced.   a     =  over  19g's  is  required  to 
v  max  °         1 
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to  obtain  the  mean  miss  distance  below  15m  in  case  1  a 

case  2.   In  case  5,  the  mean  miss  distance  does  not 

below  20m. 

Figures  5.5  and  5.4  the  variations  of  the  mean  miss 

distances  as  missile  maximum  roil  rate  varies.   [n  scenario 

1,  for  case  1  and  case  2,  a  mean  miss  distance  of  below  10m 

can  be  obtained  when   P      is  greater  than  5  rad/sec. 

m  a  x     ° 

case  5,  one  has  a  mean  miss  distance  of  below  16m  with 

P      greater  than  6  rad/sec.   The  trend  of  the  mean  miss 
max   to 

distances  is  almost  constant  over  certain  range  in  values 

of   P     .   The  variation  of  the  mean  miss  distance  below 
max 

this  range  is  a  tenth  of  the  magnitude  comparing  with  the 

variation  of  the  mean  miss  distances  as  a  function  of 

maximum  acceleration.   It  is  concluded  that  for  scenario  1, 

the  mean  miss  distance  in  case  1  and  case  2  is  not  effected 

by  the  roll  rate  if  the  roll  rate  is  above  5  rad/sec.   In 

case  5,  higher  roll  rate  limit  can  reduce  the  mean  miss 

distance.   For  scenario  2,  the  mean  miss  distances  of  case 

1  and  case  2  are  like  the  trends  for  scenario  1.   However, 

the  mean  miss  distance  of  case  5  has  a  minimum  of  26m  at 

P     =4  rad/sec,  then  increases  up  to  36m  then  decreases 
max  '  r 

slightly.   This  trend  ma}-  be  due  to  the  effect  of  the  roll 
rate  lag,  which  is  caused  by  the  increased  maximum  roll 
rate  and  by  the  underest imat ions  of  the  state  variables  in 
case  3.   The  other  characteristics,  i.e.,  the  mean  miss 
distance  decreases  with  increased  maximum  roll  rate  limit 
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and  for  scenario  ! ,  case  2  shows  less  near  miss  di    lc 
than  case  Z,    but  for  scenario  2,  the  opposite  tren       s, 
can  be  explained  with  the  same  reasons  as  those  of      Eirst 
ana Is /is . 

The  variation  o f  the  mean  miss  distances  with  the  time 
cosntant  as  a  parameter  is  shown  in  Figures  5.5   and  5.6. 
For  both  scenarios,  one  has  a  flat  plateau  for  low  values  of 
the  time  constant  and  a  sharp  rise  with  increasing  calues  of 
the  time  constant,  i.e.,  for  scenario  1,  Figure  5.5  shows 
the  curves  of  the  mean  miss  distances  slightly  increase  up 
to  the  time  constant  of  0.9  second,  for  scenario  2,  the 
curves  up  to  0.5  second  are  plateau,  then  for  both  scenarios 
the  curves  rise  sharply.   For  scenario  1,  the  mean  miss 
distance  of  case  2  is  less  than  that  of  case  1  up  to  0.5 
second  time  lag,  but  above  0.5  second  lag,  the  results  show 
an  opposite  trend.   Also  over  this  time  lag,  the  curves  of 
case  3  for  both  scenarios  are  not  sensitive  to  the  variation 
of  parameter  up  to  the  time  constant  of  0.7  second  for 
scenario  1,  0.9  second  for  scenario  2.   The  general  trend  of 
the  curves,  i.e.,  as  the  time  constant  becomes  longer,  the 
miss  distance  becomes  larger,  is  due  to  slow  system  response 
From  the  figures  showing  the  results,  the  time  lag  of  less 
than  0.5  second  is  necessary  to  obtain  the  mean  miss 
distance  of  the  required  order  of  magnitude. 

Figures  5.7  and  5.8  show  the  variations  of  the  mean  miss 
distances  as  a  function  of  one  sigma  angle  noise.   For 
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scenario  1,  as  shown  in  Figure  5.",  the  m  .  ■   am  mi 

tance  is  obtai     for  the  six  measurement  cas< 
miss  distance  being      .    Lest  for  the  two  me  ; 
case.   Above  about  the  angle  error  of  0.5  degree  the  mi: 
distance  is  not  a  strong  function  of  the  an  L<  error.   For 
small  error  values,  i.e.,  less  than  (K2  degree,  the  curves 
seem  to  be  approaching  each  other.     ■  the  scenario  2,  the 
miss  distance  is  a  strong  function  of  the  angle  error.   For 
the  two  scenarios  (Figure  5.7  and  5.8),  the  two  measurement 
case  lias  similar  results  up  to  an  error  of  0.4  degree.   All 
cases  show  a  strong  rise  in  the  miss  distance  with  increas- 
ing error.   The  four  measurement  case  seems  pasticularly 
sensitive . 

The  variation  of  the  miss  distances  as  a  function  of 
missile  velocity  are  shown  in  Figures  5.9  and  5.10.   During 
simulation  the  initial  stand  off  distance  was  increased  to 
keep  the  engagement  time  the  same.   One  important  aspect  of 
Figure  5.9  is  that  represents  an  opposite  trend  of  the 
vatiations  in  the  mean  miss  distance  to  the  general  trend 
of  the  variation  in  the  mean  miss  distance.   Under  the 
missile  velocity  of  700m/s,  the  mean  miss  distance  is  less 
than  10m  for  case  3,  from  15m  to  12.5m  for  case  2,  from  15.7m 
to  14.5m  for  case  1.   However,  the  curves  for  scenario  2 
show  the  usual  trend,  i.e.,  the  mean  miss  distance  of  case  1 
is  the  smallest  and  that  of  case  5  is  the  largest  among  the 
mean  miss  distances  of  all  three  cases  at  a  value  of  the 
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given  parameter.   For  both  scenarios,  the  general  trend 
the  curves  slowly  increases  as  the  missile  velocit; 
increases,  except  for  one  curve  of  case  1.   It  la- 

rized  the  foregoing,  considerations  as  follows:   As  the 
values  of  the  missile  and  target  geometry  components  become 
smaller,  i.e.,  relatively  less  effect  of  neglecting  the 
higher  order  terms  in  the  first  order  filter  and  less 
contamination  of  the  measured  vectors  to  noises,  the  mean 
miss  distance  can  be  enhanced  with  the  estimator  of  the  six- 
measurement  vectors.   As  the  estimator  algorithm  becomes 
simple,  the  trend  of  the  mean  miss  distances  is  less  sensi- 
tive to  the  target  geometry. 

Figures  5.11  and  5.12  represent  the  effect  on  the  mean 
miss  distance  of  the  total  engagement  time.   For  simulation 
the  initial  stand  off  distance  was  increased  to  keep  the 
constant  missile  velocity.   The  general  behavior  of  the  miss 
distances  of  all  three  cases  slowly  decrease  as  the  engage- 
ment time  increases.   For  case  1,  the  mean  miss  distance 
slowly  decreases  or  is  almost  constant  at  below  10m.   In  the 
other  two  cases,  there  is  more  variation  in  the  miss  distance 
but  the  general  trend  is  still  downward  with  increasing  time 
to  go.   Note  that  as  time  to  go  increases  for  a  given   T, 
one  has  more  characteristic  time  interval  available  reflected 

in   T  /t     .      The  mean  miss  distance  therefore  should  be  less 
go 

as  the  total  engagement  time  increases.   This  fact  is  veri- 
fied from  both  figures . 
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Figures  5.13  through  5.18  >]   ,  the  variation 
in  miss  distances  of  ill  three  cases  for  side- 
engagement  (scenario  3)  as  a  function   :  each  para     r. 
The  large  miss  distances  resulting  '  - m  this,       r :  d  3 
motivated  its  separate  consideration,   General  trends  of 
curves  are  similar  to  the  other  scenarios  but  with  higher 
miss  distance.   This  scenario  is  a  strong  test  of  the 
system.   In  this  scenario,  the  control  laws  are  subject  to 
the  error  due  to  the  limitation  of  the  small  angle  assump- 
tion and  the  higher  maximum  acceleration  capability  is 
required  due  to  the  large  variation  of  the  PZC-miss  in 
Y-  and  Z-direction.   Thus  there  are  some  unexpected  trends. 
Figure  5.15  shows  the  variation  of  the  mean  miss 

distances  for  each  case  as  the  a     increases.   Although 

max  & 

the  missile  velocity  is  less  than  that  of  scenarios  1  and  2, 

the  maximum  allowable  acceleration  is  increased.   As  shown 

in  figure,  the  mean  miss  distance  for  case  1  is  11m, 

for  case  2  is  10m,  for  case  5  is  15.5m  only  at   a     =  23g's 

-       max 

The  figure  shows  the  miss  distance  variations  in  this 

scenario  are  more  sensitive  to  the   a     than  in  the  other 

max 

two  scenarios.   The  mean  miss  distance  of  below  40m  can  be 

obtained  at   a     =  over  19g's,  i.e.,  more   a      is 
max  &  max 

required  than  in  scenarios  1  and  2.   This  trend  of  the  mean 
miss  distances  can  be  explained  with  the  larger  variation  of 
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the    state    vector    in      Y-di  r    .tion   ivhi   :]  .-;       ' 

Le  In   control     :o     land  on     . 

As    shown    in    ;  Lgure    5.14,    the    variation? 

distances    of    case    1    and    2    as    a    function    of      pa 

constant    above      P  =4    rad/sec.       That    of    case    3,    ho       rev, 

max  '        ' 

continuously  decreases  up  to   P     =6  rad/sec,  thee,  i 

max 

almost  constant.   For  scenario  3  one  needs  a  i     urn  re  1  . 
rate  of  about  5  or  6  rad/sec  to  have  an  effective  BTT 
missile,  which  is  higher  than  in  scenarios  1  and  2. 
Figure  5.15  shows  the  variations  of  the  mean  miss  dist 
as  a  function  of  time  constant.   Up  to  the  time  constant  of 
0.7  second,  the  mean  miss  distance  is  less  20m.   For  all 
three  cases,  comparing  with  the  results  for  scenarios  1  and 
2,  the  figure  shows  the  curves  of  the  mean  miss  distances 
are  less  sensitive  in  this  scenario.   The  target  accelera- 
tion vector  lies  in  same  direction  as  the  missile  velocity 
as  shown  in  Figure  5.4.   Because  the  components  of  the 
relative  velocity  are  major  components  in  computing  the 
control  commands,  the  more  correct  control  commands  mav  be 


computed  due  to  the  small  uncertainty  in   M,  p   and   M 


bz 


components.   This  results  in  smooth  variation  of  the  mean 
miss  distance  curve.   The  trend  of  the  miss  distances  as 
the  one  sigma  angle  error  increases,  shows  some  different 
characteristics  from  the  trend  for  scenarios  1  and  2  in 
Figure  5.16.   The  mean  miss  distances  for  all  cases 
increase  continuously  as  the  noise  input  increases  up  to  th< 
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one  sigma  error  of  about  0.4  degree.   \bove  the  one  sigma 
error  of  '■'■>  .  \  8  degree  ,  case  1  d         then  rises  : 
case  2  continues  decreasing  slowly,  case  5  increase 
continuously.   The  variations  of  the  mean  miss  distances  in 
scenario  1  are  almost  plateau  and  in  scenario  2  are  almo  I 
constant  up  to  the  one  sigma  angle  error  of  about  0.4  degree 
then  rises  sharply  as  shown  in  Figures  5.7  and  5.S.   rhis 
reason  can  be  deduced  from  the  fact  that  as  the  one  sigma 
angle  error  becomes  large,  the  same  magnitude  of  the  two 
state  vectors  in  different  direction,  i.e.,  relative 
distance  in   X-direction  and  relative  in  Y-direction,  having 
the  same  order  of  magnitude  ma}'  be  contaminated  to  the  noise, 
on  the  other  hand,  the  other  scenarios,  only  the  components 
in  the  X-direction  being  relatively  large  are  contaminated 
to  the  noise.   From  this  fact,  it  can  be  concluded  that  the 
missile  in  a  side -engagement  case  is  more  sensitive  to  the 
angle  error  than  the  other  scenario. 

Figure  5.17  shows  the  variations  of  the  mean  miss 
distances  as  the  missile  velocity  increases  as  a  parameter 
(i.e.,  holding  the  total  engagement  time  of  5  seconds  and 
changing  the  relative  position  of  the  target).   Up  to  the 
missile  velocity  of  700m/s  the  curves  of  the  mean  miss 
distances  are  plateau,  after  that  velocity  the  curves  rise 
sharply.   Figure  5.S  shows  the  variations  of  the  mean  miss 
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distances  as  a  function  of  the  total  engagement  time   I.e., 
holding  the  constant  velocities  of  the  missile 
target  ana  changing  the  relative  position  of  the  target; 
scenario)  . 

figure  5.  IS  shows  the  mean  miss  distances  rise  sharply 
over  the  engagement  time  of  6  seconds.   This  is  somewhat 
unexpected  and  may  be  an  artifact  of  the  implementation 
procedure.   In  other  scenarios  the  mean  miss  distance  as  a 
function  of  the  missile  velocity  increases  slightly  as  the 
missile  velocity  increases  and  that  as  a  function  of  the 
total  engagement  time  decreases  slowly  with  increased 
engagement  time.   from  above  analysis  one  has  a  conclusion 
that  the  computed  control  commands  should  converge  in  the 
final  time  as  the  computed  commands  of  the  two-  four- 
measurement  case  shown  in  Figure  3.32  and  Figure  5.54  to 
obtain  the  low  mean  miss  distance.   However,  it  can  be 
expected  in  this  scenario  that  the  computed  control  commands 
do  not  converge  possibly  due  to  the  violation  of  the  small 
angle  assumption  in  deriving  the  equations  of  the  control 
commands  (I.e.,  in  this  scenario  the   M.    component  is 
large  in  Equation  (5.7),  that  causes  the  large  mean  miss 
distances).   However,  in  the  case  of  relatively  small  magni- 
tude of  the  missile  and  target  geometry  components,  the 
error  caused  by  the  violation  of  small  angle  assumption  is 
relatively  small  comparing  with  the  errors  caused  by  the 

/  it        o  > 

contaminated    of  the  measured  vectors  to  noises, 
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limitations  of  the  control  inputs,  an  I   he  estimation 
tSic  state  vectors  using  first -order  filter.   Ph  ; 
verified  with  the  results  chat  the  mean  miss  d: 
11m  for  case  1,  10m  for  case  2,    14m  for  case  5  at  the 
missile  velocity  of  600m/s  and  the  engagement  time  of  5 
seconds,  as  shown  in  Figures  5.17  and  5.18. 

The  final  Figure  5.19  shows  the  variations  of  the  . 
miss  distances  for  case  5  of  all  three  scenarios  as  a  func- 
tion of  the  one  sigma  angle  rate  error.   Ihe  result  repre- 
sents that  the  variations  of  the  mean  miss  distances  due  to 
increased  input  values  of  one  sigma  angle  rate  error  are 
almost  constant  for  scenarios  1  and  2.   For  scenario  5,  the 
mean  miss  distance  varies  at  constant  rate  of  inclination. 
This  slope  of  the  curve  is  relatively  small  comparing  with 
the  slopes  of  the  mean  miss  distance  curves  as  the  other 
parameters  change.   From  this  analysis,  it  may  be  concluded 
that  the  exposure  of  the  measured  vectors  to  the  angle  rate 
error  noise  is  neg legible  in  the  case  of  present  study. 
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VI .   CONCLUSIONS 

A  simple  but:  effective  biased  guidance  lav;  and  t  ree 
extended  Kalman  filter  equations  have  been  deri  ed  from  : 
optimal  control  theory  for  a  bank- to- turn  missile  with  zero- 
lag  and  with  0.5  second  time  lag  autopilots  for  pitch  accel- 
eration and  roll  state. 

The  equations  of  motion  are  linearized  around  the  present 
orientation  of  the  missile  with  the  assumption  of  small 
future  roll  angles.   During  computer  simulation,  the  optimal 
control  laws  and  estimators  were  tested  separately  invoking 
the  separation  theorem.   In  the  simulation  of  the  system 
three  optimal  estimators  were  used,  i.e.,  estimator 
with  two  measure:.! cut  vectors,  with  four-measurement  vectors 
and  with  six-measurement  vectors.   The  system  has  been  eval- 
uated for  three  scenarios,  tail  chase  engagement,  head  on 
engagement  and  side  approach  engagement. 

From  the  analyses  of  the  simulation  results  in  Chapters 
3,  4  and  5,  it  can  be  concluded  as  follows:   The  control  law 
was  successfully  simulated  for  a  hypothetical  bank-to-turn 
missile  with  pitch  acceleration  and  roll  rate  autopilot 
within  the  imposed  limits  on  three  scenarios.   Miss  distances 
with  zero- lag  were  negligible  for  all  three  scenarios 
(below  0.5m) .   For  the  time  lag  case  one  had  miss  distance 
of  5.9m  for  scenario  1,  0.8m  for  scenario  2  and  32.9m,  with 
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0.5  second  lag  and  with   arnnx  =  20g's.   By  Li irposi  ig  a 
higher  maximum   g   of  2  3  on  the  scenario  3,  the  miss 
distance  was  reduced  to  below  5m  miss  distance. 
expect  a  high   g   requirement  for  a  side  engagement  situa 
tion  as  in  the  case  of  scenario  5.   It  is  evident  that  the 
missile's  tracking  ability  against  the  target  is  very 
sensitive  to  the  missile  and  target  geometry,  the  missile's 
manuevering  capability  is  also  a  very  important  factor. 
Optimal  control  laws  applied  to  the  side  approach  engage- 
ment place  a  severe  strain  on  the  system  from  the  viewpoint 
of  the  small  angle  approximation.   Increasing  the  number  of 
measurements  did  not  result  in  increased  performance  in  the 
sense  of  smaller  miss  distances  for  the  missile  except  in 
isolated  cases.   This  may  be  due  to  the  increased  com- 
plexity of  the  system  as  the  number  of  measurements  was 
augmented.   Further,  in  this  stud}'  only  first  order  extended 
filters  were  implemented.   It  is  possible  that  better 
performance  could  be  obtained  at  the  expense  of  increased 
computational  load,  by  using  a  second  order  extended  Kalman 
filter. 
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APPENDIX  A:   PROGRAM  LISTING 

This  appendix  provides  listing  of  the  computer  prog] 
used  in  the  present  study.   Only  one  program  for  the  six 
measurement  case  is  provided.   Except  for  a  few  differences 
in  the  subroutine  filter  the  programs  for  the  A./o  other 
cases  are  identical  to  the  program  included  here. 
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i;      ))  ,  £  Al  K  (203)  ,  TGI  (200)  ,TG_  (2  2  0) 

1  (200)   ,G4  i  (20  J)  ,  G5  1  (203) 

CC,  ,UH   (2CC)  ,  U5  (23J) 

CO    ,U9  (200) 

CO)  ,£■;  (2  )0)  ,  E5  (230) 

CC)  ,E9  (2CC) 

CO)  ,?«   (2J0)  ,  P5  (200) 

CO)  , ?  9  1 2  0  C ) 

ZMISS  (  53)  .DMI3S  (50) 

ZO,CTX  ,  CI T,  CI  2, 02,  ISPIN.TACC 
}32,;>3  2  ,  2  1,  £2 


»»iM«»****4**t*HM**S***?+***44*?*?*t*»***** 


I  N  F  U I     D  &  1 2 


DII=.0 
HCASE= 
N£AX=1 
KEEINT 
LPP.IN2 
IIICT= 
IPIO=0 
ESIO=0 
PHI 0=0 
V£0=10 
X  H  C-=  0  . 
XEO=0. 
ZKC=0. 
XI0=25 
Y  I  C  =  0 . 
ZI0=-1 
V 1 X  C = 5 
VTYO=0 

vazo=c 

E1=.00 
B2=1.2 
ATXO=0 
Al  i  C=u 
ATZO=0 
C 1 X = ..  0 
C  T  X = .  0 
C1Z=.0 
S£l=5. 


5 
1 
10 

=  1 
=  5 

1 


30 


00. 

00. 
CC 


325a 


,-*S.8 


5 

5 

*S.( 

20. 

026 

C87 

026 

087 


521  = 

RZ2=.ui 

E23=.0( 

RZU=.Gi 

HZ5=3. 

RZ6=6. 

SB1=.0< 

SH2=.0 ' 

SE3=.0! 

sai»=_Oi 

SE5=3. 

SB6=6. 

SFCS=3. 

SVEL=6 

S«CC=9. 

1SPIN=. 

TACC=. 5 

A«AX=2C.=»S.3 


I  026 
iC37 
0  26 
i  08  7 


.  5 


15. 


4HIN=- i.  »5 

E  K 1 X =6  -   >  £ 

Q  F  S  C=0  . 

ISEED=675 

c 

c 

******  *-**** 

c 

c 

ISITIALIZI 

c 

j**;,S***.»«x«  **********   <  -    -*jrJ<^- 


S  ,   «  ,  F      I 

DC     1:0     1=  1,  ) 

DO     ICO    J=1,9 

DEI  (I,  J)  =  0. 

I:  |I.3Q.JJ     DEI  |I,. 

A(I,J)=DEL(I,J) 

:  (i.J)  =  o . 

10  0       CCKTI2JUE 
VIC       C0NTISU3 

DC  130  1=1,2 
DC  12  J  J=1,2 
a  II,  J)   =0. 

120     ccsrikuE 

13C       CCSTINU3 


B  (1,1 
E  ( 2  ,  2 

R  t3,3 

a  (4,4 

S  15,5 

a  ( c ,  o 


=  SE  1**2 
=  S  r  2  *  *  2 

=  3  F  3**  2 

=  5  F  -  ;'  *  2 

=  2  ?  6  -1  *  2 


a  (1,4)  =Dn 
p.  i2,S   ---d:t 

A  (2,o)  =DI1 

CAIL    FUNC  (    SAT, CI, CI Tf?1  , F2 , ?3 , Q1 1 , Q  12 , C. 13 , Q22 , 3;23 , Q33) 

II  #2)  =-°l 

(«,7)  =F2 
(7,7)  =F3 


(2,3)  =F1 
lb, 2)  =  F2 
|c,d)  =F3 


IE, 8)  =s 


=F1 


=  C  1/ 


IS, 3)  =., 
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13a 
5 
6 
7 


Q  (5,6)  =Q22 
C  (9,9)  =  *33 

KErrE(6,5) 
.'RITE  (6,6) 

dc   u-.  1=1,9 

WRITS  (6,  7)     i  (1,1)  ,Q  (1,2)  ,5(1.3)  ,Q  (I,  4)  ,C  (1,5)  ,3(1,6}  ,Q  (1,7 

'  ,  "   !  I  J  )    ,  C  ( I  ,  J  ) 

cc:*:::;  U2 

FCR3AT  (MM 

":5X,  'C61GISSI.    2    MATRIX'//) 


FC  R M  A I  (  9  ?  1 4  .  '6 ) 

fcekat  (//-< x,  ' ;   KaiRix   «iih   ?s:u:;    jjcish, 


J?3U=«  ,75.3//) 


ACL  FSF.UIC 


3AT2IX 


F11=  (JPSU**2)  *  (CT  1**4)  /4. 

?i2=to?sL'**2)*(D'n**j)/2 
F22=  (Cjpsu**2)  *  (.: ;  i**2) 

C(1,1)=C(J,1)  *|11 
Q  (2,2  =Q  |z,2  +F11 
C  (J, 3)  =0  u,J)  *r1 1 

Q(c,b)=Q(6,6)+F22 

Q  (1,4)  =Q  (1,4)  +  F12 
C(2,d)=QU,d)+F1.<: 
2  (3,6)  =Q  (3,o)  +F12 

Q  (4,1)  =Q (4,1) +F12 
C  (5,2)  =(J  (5,2]  -  .-  1  - 
Q(€,3)=U(6,3)+F12 


:«RIT2(6,g)   ,?H 
CC  135  1=1,9 

"  (I 
>) 


■*RI1SJ6,7)  2  (1,1)  ,C  (1,2)  ,  £  (I  ,,,, 
,  C  <I,."1)  ,  £  II,  9] 


(1,5)  ,  3(1, °)  ,s2  (1,7] 


135   CONTINUE 

C  C     15  0    1=1,9 

CC    1-J3     J  =  1 ,  9 

AT  (I,  J)  =A  (J,  I) 
140       CCXTINUE 
ISC       CCSTI2J02 

5.31X2(6,5) 

SIART  CASH  LOC£ 

DC  950  111=1 , KCASE 

i.nitiai.::z 
s=c 

K=KPRINT-1 
L=L? RI NT -1 
ISTCP=0 

DT=DIT 

IFfN'CA  SE.FQ.  1)     GC    1C    16  ) 
K£Si:iT  =  lC  jOO  3 

LE8iNT=icccoa 

1 1 LOT—  0 
K  =  C 
1  =  0 
16C       CCMTISOE 

Y(1)  =0. 

Y  (2)  =THTC 

Y  j3   =psi c 
i  |a)=vao 

■i  (S)  =xso 

Y  (5)  =  YMO 
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i  (7)  =2  210 
•£  (£]  =V1XC 

i  (S)  =v  :yc 
X  (10)  =v 


mi 

I    12) 

X  (13) 

?  115) 
X(16 


=  X  :  j 
=  ;  l  C 

=  P  H 1 1 

=  C*. 


DRAW    2  A  :«'  C  C  S 


17  C 

180 


190 
2CC 


CALL 

c;;l 

CALL 
CALL 
CALL 
CALL 


l,  >i  c  ?  iv  c;nc  , '. -.  , :  ;ja  x ,  l,  i 

i.  NC  £  a  { LS  c  £  C  ,  1 2  ,  i  MA  X  ,  1 ,  1 
LN05S  (ISE.E!  ,  *3  ,1  MA  £  ,  1,1 
L.NC  EH  (ISrEC,  *U,  :.:'.AX,  ' 


SO  E  M  (LSI; 

t:,c; 


INITIALIZE    ?     Mi 

CC    180     1-1,9 
C  C     1  7  j    J  =  1 ,  9 
E  1 1 ,  J )  =  0  . 
CCNTINLE 

c  c  :>:::;•; : 

E  (1 ,  1)  =S  ICS**2 
?  (2,2)  =SFCS*  '2 
E  [J, J)  =5  £CS**2 
2  (4,4)  =S  V  EL  *  *  2 
P  (5,5  =SVEL**2 
F  |b  ,6)  =SVEL**2 
?  (7,7)  =SACC**2 
E  (8  , 3 )  =S  A C  '  - 
P  (S,9)  =SACC**2 

DC    200    1=1,5 
lC    190    J=1,2 
GAIN  (I, J)  =0. 
CCMIXOE 
CONTIN US 


n  ,  .*  SA  .'.  ,    I ,   I 

i : x 


70C 


£  I A  I!  I     IB 

CONTIS  (IE 
S=fi  +  1 
K = K  +  1 

L=L+1 


SCICE:     LCCE 


C5EINZ     IXTEGSATICS     VARIABLES 

T=I(1) 

7  R  I  = Y  (  2 ) 
ESI=Y (J) 

v  <v.  =  y  { .i ; 
xs=f 

YM=3f  (bj 

2*=Y  (7) 
V1J(  =  Y  |  B 
VTY=Y    9i 
VTZ=Y    10) 
XT=Y  (  i  1) 
Y1  =  Y (1 25 
ZT=Y (13) 
PHI=Y (14) 
£ fi-  Y  {  T  5) 
A  H  =  Y  ( 1  6 ) 

INE3TIAL    10    BCDY     2X1 


IHAKSFOSKATION 

A11=DCO£  (1HT)  *ECC£  (ESI) 
A12=CCC:,-  (THT)  *  E  S  I  :;  (ESI) 

A13=-D£I.\  i  ;h  r) 

A21  =  D5i:i  (EKI)  *D£IB  (THT)  *DCOS  (ESI)  -DCCS  (EHI)*  DSIII  (PS  I) 
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A22=DSIN  (PHI)  *C=I!i  (IHT         '        .'  (  SSI )  -DC  CS  i  I  £  £ )  *  DC  3S  I 
A23=33  Hi  (EHI)    ■• CCCS  (I 

a31=dcos  i  ■:    L)     :_:.-:.•:,  *dcos  (ESI)  +  33  [j;<phi)  *  331:1  (?si) 

132  =  DCC3  >  *C£IS  (TfcT)  *CSIS  (ESI)-DSIfc  (£HI)*DC     , 

A33=DCC3  |PHI)  4  CCCE  (TEX) 


vkx=ai 1*VK 

•  3*=A1  2*,iM 

vez=ai  J*va 

c 

c 

CG2PUTE    I? 

c 

BX=XT-XM 

g  Y  =  XT-  v  ■' 

EZ=ZT-ZH 

UE    CCXECSESTS    01 


7ECI0B 


VEX=VTX-VKX 

VR^VT*-  SI!  £ 
V5Z=VT2-V£Z 

aix=Axxo*EExs  i-c~ 

ft  i  2  =  A  £  :  3*33  X?  (-C  :  1*1) 
4TZ=ATZ0*CEX2  (-CTZ*T) 

X2  (  1)  =  BX 
X2(2)  =B'i 

X2  13)  =32 
X  2  { 4 )  =  V  ?.  ;( 
X2  i  5)  =  VBY 

X2  (6)  =vh  : 

X2  (7)  =  ATX 
X  2  ( 8 )  =  A  T  X 
X2  <9)  =  ATZ 

1GC=-{5X*VHX+EX*VEY  +  EZ*VHZ)  /  (7SX**2+ Vra**2+'/aZ**2) 

£C£;J.    MEASD3H:iE5I     VEC1C: 

S  1  =  33  2ST  (SX**2+SY**  2) 
S  =  D  SQ  3 1  ( £  2  * *  2  -£':'-  *  2  +  S  Z*  *  2 ) 
3ET=  (3  X*VEX+S  i*VRY*SZ*VaZ)/S 
THTS=-DATAN2  (SZ,S1) 
P£IS=DA2AN2  ( 3  :  .£>) 

T£TSD=  (  (-CSIM  (TKIS)*CCOS  (PS  IS)  *SX*VB  X)  -  (3  3  IN  (THIS)  *DSIN(PS 
*RY)+ (DCOE  (TH1SJ>S2*V5Z)  )  /  (S*S) 
ES13L=  (  f-ESIN  (Sill)  *Si*VRX)  +  (LCC3  (PS  IS)  *ST*VaY)  )  /  (5*S*  333  3 
BEIS=3 
BDI£D= (SX*VHX+ £ i*SE Y+SZ* VSZ) /S 

Z  I  1)  =IfiIS    03LE  [il  (N))*SZ1 
Z  1 2)  -  r  HT  £  E  +  C  E  L  E  (  «  2  ( v.  i  )  -  3  3  2 

Z  (  3 )  =?  31  5    +-D  3 1 E  i  .  2  ;'..))*  S  Z  3 


13)  *  3  ":  •  Y 
(THIS)  ) 


Z   (4 


.2(S))*SZ5 


Z  (5)  =  2EIS     fCsL:  ( 

Z  (6)  =8DI£E+D3LE  (h2  (il) )  *SZ6 

IHITIAIIZE    SIA1E    T«£C10?. 

IF  (N-GI.  1)    30    TO    750 

CALL    LHCSK(ISEEE,Sn,-6,l     1  ) 
X  |1)=RX+E  ELE  ( JiK  (  1) )  *SrOS 

X  (2)  =R  X+  C  ELS  ('.'.>  (  2  )  )  *  SPOS 
X  (2)=aZ+LEL£  (Vifl  (3     )  *  S20S 


75  i 


7.  |4)=V  HX+3313  (Si  -.  (U) 

i  (  5 )  =  7  B  X  +  C  3 L  £  ("» '*  (5 

X  (6)  =VHZ+CDLE  |  vi  •  (6 

X (7)  =0  . 

X  (8)  =  5. 

X(S}=0. 

CCKIINUE 


♦  SVEL 

v :  i. 

7  3L 


LI.  Li 

:  L  , 


- 


C12  =  DC0£  ilHlS)  *0: 

C13=-DSIK  [TH    ..  ! 

XIC3  =  D  11*X(1)+C12*X(2)+D1 


:X(3) 
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GENERATE    GuI^ANCE    CCf.MASDS 

CALL    3  DICE  (X,IGC2  ,AC  ,PC, CI) 

IF  (AC. GT.+AaAX)  AC  =  A  • 

If  (AC.  LT.  -AMIS)  JC--. ''.:!'; 

IF  (PC  GT.+t  HA  :<)  PC=E  KA  ! 

I?  |EC.  LT  .  -::::.:;;  [C=-EKA  < 

I?  ITSPIN.IE-  J.  )     r:"=^C 
I  r  ( T  A  C  C  .  L  E  .  0  .  )      A  fi  =  A  C 


T0?.2     VABIA31 


? C  a     E LO  - 


P  H ID=? 

AAA  [Ml 
EE2  («) 
B  A  N  K  ( N 
TG1  (>!) 
1 G  2  ( :i ) 
0  1  <N  = 
U2  (N)  = 
U3  (.,)  = 
U«  (»)  = 

::  5  i  .s )  = 

U6(J!  = 
07  |!li  = 

Gt(.N)  = 
U9  |N)  = 
El  ()•')  = 
E2  (N)  = 
E3<N)  = 
E-4  ;:,}  = 
H5  (N)  = 
•■  [K]  ■ 
2  j  (N)  = 

EeiM)  = 

2SJS)  = 

E  1  1=05 
P22=?5 
E33=DS 

P<44  =  Qo 

E55=0S 
P6c=0S 
E77=05 
PS8=DS 
E99=D5 
PI  (N)  = 
£  2  |!l)  = 

?3t!,;i  = 

EU  (N)  = 
to  (N)  = 
FS  |N>  = 

PS  (V)  - 
G1  1 
G21 

G3  1 
G  4  i  ( :< ) 
g  5  i  ( y ) 


LIKE    3  If 


HI*57.3 

=  AC 
=  £  C 

J"  =  ehid 

=TGC 

=  TGC2 

ax 

?.Y 
7  3  Y 

v  a  z 

a:  : 


lis! 

X  (6) 

lie7! 

x  I s ) 
Cat 

sai  i 
;ai 

;  -:  2 


(1 


(K. 


.  :•  I  ( P  ( 6 

C31  I?  (7 

caifP(d 

P1  1 

i    - 

P33 
P<4  a 
F55 
?6c 
?7  7 
F8  8 
?99 

=gai:j  (1,i 

=GAIN  (2,1 
=  GAIN (3, - 
=GAIN  [4  ,  1 
=  GAIN (5, ' 

LINE    E; 

T .  XEE I S 

=*5 


,  1)  ) 

;r); 


fT? 


MCASJ 


D 


K  =  C 

THTD=?HT*57.3 
?£ID  =  ?  SI^57 .3 
PHID=PHI*57 
WHITE  [5,  1C] 
WHITE (6 ,  .0) 
HHITE (6, 2C) 


)     GC    TC    250 


"i 

3 

I,£,XLCS  -IGG-TG02  .AC.AH.  EC,  EM- Pi 

T, EX,SY,SZ, /BX,   7SS,7RZ, ATX,AIY,ATZ 

I,X(1)  ,J  (2)   ,1  (3)   ,.1(4)  ,X{5)  ,X{6)  ,X(' 


rz 

X(7)  ,x{8)  ,  -,:  ;j) 
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10 

20 
25C 


30C 
3C 

•4  0 

3dC 


U0! 


50 


iSi::  (6,  2C)     r,E11,E22,?33,=>i   |,E55,?6  6,£77,E88 

FC5MAT(/1CF13.3) 

F  C  £  a  AT  ( 1  0  f  1 3  .  2  ) 

CONTINUE 


15  IL.LT  .LESI  NT) 


L^J 


TC    3  50 


BBIT2(6,3C)     : 

DC    300     1= 1,9 

KB  I  IE  (6,  i*C)      3AIK  <I#  1)  ,SAIS<I  ,  1)  ,GAI'J  U,  1)  ,  GAI>i  (I,  1)  ,  GAIN  (I,  1) 


FORMAT  (//S7X,  'GAI2> 
F  C  E v :, :  [52X,6F13.5) 
CCJiTIIi  US 


93  0 


K  A  IH 


(T  = 


,  £  3  .  . 


)  V) 


CHECK     FOB 


I S  A 1 I C  K 


STATE     VECTOR) 


IF  (SDT  .GE.G.  0  .CS  .:gc  -LZ  .  0  .  3)     GC    TC    900 

IF  |IGO. GT.iT)      GC    IC     UOO 

CT=TGO 

isicp=  i 

CCNTI?IUE 

INTEGBAIE    TRAJECTORY    ONE     5TEr     8HEAD 

CALL    INTEG  (X,£) 

IF  (ISTOP  .GT.O)     GC    TC    9  10 
IF  (N.  SE.  :.  .MX1      CC    IC     300 

EXECUTE    SALMAN    Fill  Ft    (UPCATi 

CSII    FILTER (X,E,E,Z,GAIN) 

GC    13    70C 

IEESIHAL     EBINT     A  .\  C    E  LOT 

CCSTIttUE 

t=y{1) 

CX=Y  (1  1)  -2(5) 
DX=I  (12    -Y    6 

CZ=i|1j|  -Y  (7) 

CM=DSQi?T  |EX**2+CY**2+DZ**  2) 

XfISS (III)=DX 

ZMISS (II1)=DZ 

CMISS  (iiiHdm 

SRIIE  (  6 ,  £  C)      I ,  E  X  ,  C  Y  ,  Z Z ,  D  M  .1 II 

FCfiMAT  (//    SX,  (I=  '  ,F  10.  J  ,  5  .< ,  '  DX  =  '  ,F  10  .3.  5  X  ,  '  D  Y= '  ,71  J.3,5X 
ic,,DZ=,,F1C.3,5X,,CISS  =  ,,F10.3,5X,'CASE='/I7) 


IF  (IPLOT-NE. 1)     GC    TO    950 

CALL    P  LO  TG  (TT  ,  E  1 ,  N  ,  1  , 1  ,  1  , 
*,11,0„.0.,0.,C.,5.,5.) 
CALL    ?LOTG(ri,U1/N,2,1,Q, 

*  ,11,3.,0.,().,C..3.I5.) 
CALL    PLCTG (TT <Ez,S,1,1,  1, 

*,11,O.,0.,0.,C.,3.,5.) 
CALL    PLCTG  (TT  .  Uz.  S  ,  2  ,  1,  0  , 

*  ,  1  1  ,  0  .  ,  0  .  ,  J  .  ,  J  .  ,  d  .  ,  5  . ) 
CALL    PLCTG iTT. E3  ,N  ,1  ,1,1  , 

*.  11,.;.  ,0..3.,fi.,3.r5.) 
CALL    PLOTG  (TT  .  C3  .  N  ,2  , 1  ,  0  , 

*  ,  11 ,  0 .  ,0.,G.,0.,5.,5.) 
CALL    PLOTG  (TI  ,  E4  ,H  ,  1  ,  1,  1  , 

*,  16,0.  , C-,0.  ,C.,5.  ,5.) 
CALL  PLCTG (TT ,0U,S, 2, 1, 0, 

»,16,0.r0-rJ-,C.,D.,5.) 
CALL  PLOTG(ri  .E51>,  1,1,  1  , 

*,16,0-,G..O.,C..D.,5.1  „ 
CALL  PLCTG  (TT  .  C5  ,  .\  ,  2  ,1 ,  3  , 

*  ,  1  b  ,  0 .  ,0.,0.,0.,o.,5.) 
CALL  PL01G(TT,E6  ,N',1  ,1,  1  , 


TIME  |SEC) 

TIBE  (SEC) 

lit  I  [2F.C) 

TIME  (SZC) 

II  HI  (SZC) 

TIKE  (SEC) 

TIME  (SZC) 

TIME  (SZC) 

TIME  (SEC) 

TIME  (SZC) 

TIME  (SZC) 


',10,' 

',10,' 

1  ,  1  C  ,  ' 

'  ,  10,' 

',10,' 

',10,' 

',10,' 

',10,' 

',10,' 

',13,' 

',10,' 

RX     (METERS) 

SX     (METERS) 

Tl     (METERS) 

RY     (METERS) 

?.Z     (METERS) 

RZ     (METERS) 

VRX     (METSHS/SEC) ' 

VRX     (M3TEHS/SEC)  ' 

VRY      C-iZTZRS/SEC)   ' 

VRY     (MSTS2S/SEC)   ' 

VRZ     (METERS/SEC) ' 
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*,16,0.,C.,0.,C.,5.,5.) 
CALI  ?LOIG  (?T  .  06tS,  I  ,  1,  0, 

call  ?  L  a  r  g  i  t  r .  :•  1 . :. ,  i ,  i ,  i , 

*  ,  13,0-  r0.,J.,C.,3.,5.) 
C.iU  ?L01G(TT,C7,N', 2,1,0, 

*,19,0.,0.,0.,0.,5.,5-) 

CALL  ?LOTG  (Tl  .  •   fK,1  ,1,  1  , 

*  ,  1 9  , 0  .  ,  0  .  ,  0 .  ,  fl  .  ,  5  .  ,  5  . ) 
CALL    PLJIG  (II  ,  Cc  ,:.  ,  -  ,  I,  J  , 

*,1S,0.,0.,0.,i.,,5.,5.) 

CALL  ? L ,:  1  G  (rT,£9zN, 1,1,1, 
*,1S,iJ.,3-,U-,C-<D.,     . 

C  A  Li.  -:  _c:  l-  i'i .'.',:,.,!,.  , 
*,19,0.  ,0.,0.,C.,i.,5.) 

CALL  ?LC  lG  CI,  2  ,  «,1  ,1,1  , 
*,0.,0. ,0.,0. ,5..=.) 

CALL  PL^IG  (T'I,E2  ,N,1  ,1,  1  , 
*,C.,0.,0.,u-,5.-'d.) 

CALL  ?LCTG(TT  ,  Ei,!.',  1,1,  1, 
*,C-,0.  f0.,0.  ,5. ,5.) 

c  a  L  l  s  ?  l  c  i  g  ( i : , :  y , .. ,  i , i ,  l , 
*,C.,0. ,0.,0. ,:.,5.) 
call  ?l^;  3(Trr  .t5£::,i  ,1, 1 , 

*,0.,0.,0.,U.,5.,b.) 
call  plotg  (ii  ,E6,5,1  ,1,1  , 

*,0.,0.,0.,0.,5.,S.) 
CALL    PI,G3G(T'I,E7,S,1,1,1, 

*f0.f0.,0.,0.fS..£.) 
CALL    PLO TG  (T T  ,  f 6  -  S  ,  1  , 1 ,  1  , 

*  ,    Yj  .  ,    ^  •    ,  O   .    ,  o  .    ,    _    .    f  ^   »  i 

call  ;  l  :  :  g  ,  r :  .  :  r , ;. ,  i ,  i ,  i , 

CALL    ? LC T  G  (T T  ,  A  ;  C    <i  ,  1 , 1 ,  1 

* , '  Mosa  m  a_c_l::  >1ic      (.1/ 

CALL    PLC!  G  (T T  ,  1 I. li  ,  11 ,  2 ,  1  ,  0 

*  ,  '  NOaa  .  L     ACCELEi  ftllCJi     (."!/ 

call   ?lc:g  ,n  ,eec  ,:; ,  • .  ,  ,  ! 

*  ,  ■  BCLL  52TE  I BAL/SEC)  ',  19 
CALL  PLOTG  (TT  ,  E23  ,N  ,2,1  ,0 

*,  'EXLL  BAIL  [fiAE/S£C)',19 

CALL  ?LCIG  (TT  ,  E  ?  ..  .-  .N  ,1,  1, 

*,  'LANK  A  SGLE   |CEG)  '  ,  16,0. 

CALL  ?lcig(::,ig:,::,-,i,i 

*,  'TIME  IC  GO   I  SEC)  '  ,1b,  5. 

CALL  ?L01G(TT  ,TG1  ,N,2,1  .0 
* . 'TIMS  TC  GO   (SEC)  '  ,  16, 0. 

CALL  ?LC1G(TI,G1  1  ,N,1.1  /  1 
*, 'G  (2,  2)  '  ,6,0.  ,0.,0.  ,0.  ,  5 

CALL  ?L01G  (II  ,G2  1  ,li  ,  1.1  ,  1 
* , ' G  < 5 , 2 )   ',6,G.,C.,0.,0.,5 

CALL  PLOT G  (TT,G3  1,ii,  1,1,  1 


*,  'G  (8,2  ' ,b,0.  ,C.  ,C.  .0. ,5.,D.  ) 

CALL  ?LOTG(TT,G1  1  ,N  ,  1,1,  1,  'TIKE 
*  ,  '  G  (3  ,  1)  '  ,5  ,  C  .  ,  C  .  ,0.  ,0-  ,  5  .  ,5  .  ) 


ciai  iszz) 

TIME  |S  EC) 

PI  81  (SEC) 

TI3E  (SEC) 

TIKE  (SEC) 

i :  ;•:  • 

nai  i  s  •  i  ■  i 

riKE  (SEC) 

TIKE  (SEC) 

TI3E  (SEC) 

TIME  (SEC) 


TIKE 


ri: 


(SEC) 
(SEC) 
IS  EC) 

(SEC) 

(slc; 


I  '       i 
s  c  *  * : 


(SEC 
',30 

XML  I  '-J 

I    J.    ..    I  U-v 

EC^*,)  •  ,30 

■TIM 

0  .  ,  C  .  ,  J  .  ,  0 

"TI2E     (SEC 

0. ,C.,0.,0 

, '  i i :'  e    i 

J  .  ,  C . ,  0  .  ,  5  . 
'TIKE     (SEC 

0  .  ,  0  .  ,  0  .  ,  5 

'  PIKE      (GLC 

) . ,  c . ,  d . ,  5 

1  ™    -    '«    -  /    I  7  £ 

lTI?.E     (SEC 
,5.) 

rial    (sec 


CALL    PLCTG(TT,G5  1  ,"J,  1,1  ,  1  ,'  TIME 
**.\9S6 l}11'<?.0.  ,C..Q.  ,0.  ,S.,5.) 

CALL    PLOT  (0.  ,U  .  ,?99) 


(SEC 


1  C  ,  '  73  z  ; )   ' 

10, '  \:/.  v '     '  :  - 

10,' ATX  (  iETEHS/SEC **2) ' 

TO,  '  ATY  {::£IL=S/JEC~J'.0  ' 

10,  '  '.  "•  (!   ET2aS/SEC**2)  ' 

10, '  ATZ    ,'.■::  3S/  sec  i^2)  • 
.  ■  -.i:    (as  i!Eas/ss  ;**2)  ■ 

10  ,  'SU2T  (P11) 
IC,  »SGHT(P22) 
10,     'SQRT(?33)  '  ,9 

ic ,    •  sqst  (?uu)  « , 

10,     ■  SQST  (?55)  '  , 

'  C 

' S  QST  ( P7 7 )  » , 

'SCjHI(?S8)  ' 

•SQSI  (i?9y)   • 


10, 
10, 
1C, 
IC, 
,10 

;  i  ■: 
/- 

ir 

3.) 
,    10 

,  10 

,  10 
,  10 
,  10 


.  ,  0  .  ,  0  .  ,  5  . 
*  f  J  •  t  \j »  j  b 
5.) 


,5.  ! 


95 C      CCHTINCE 

i  CCaPQTE     KISS    DISIANCE    STATISTICS 

17  (HCASS.EQ. 1)  GC  TC  975 

CALL  STAT  (XaiSS  ,  :!C AS  E  ,XB AS,XR£S) 

CALL  SIA1  iiJlLL;,'.GA5::,:-;^,  fSCS) 

CALL    STAT  (2.V;ISS,  '..CAS  E,Z3A5,  0  3KS) 

CaLLSIAl(Dai£S,NCASE,DBAo,DSl!S) 

S  SITE  (b,  ij_ 

H£ITE(6j6C)     XE8E,XEKS 

«HITE(6,"/C)     YE  A  5, 'iR  S3 

«EITE(6»60)     ZEJ»B,ZEKS 

WRI1E(6,SC1     DEAE.E5KS 
55  FCE-MAT  (//13X,  •  KISS    CISTA^iCL    STATISTICS'/) 

6C         FCEMAT (/Si,' X-CCMECSENT' ,5X, ' XEAN=' , SB. 2,5X, ' aHS=',F8.2) 
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7C       Foaa.s  r  f/sx,1  y-cc^ic:; 
30        .-■      '  •  :  !/  5X, '  :-^:c::^-:.  ■  IT 
FCEaAX  (/5.(, '  :c:.;: .   :i  . 

'97  5       CCSTINUE 

SIC? 


,  3X,  »KEAI 

,      .  ,  '  »£AN 


■  a   2 , 5  .< , 


?.as  =  ' 
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50 
103 


SCBHOOTISfi  jCIDE  (X  ,TGO,  AC,  =C,CT) 

I2EIICIT     6EAL  j  E      [1-r  , 

DIMENSION        (9) 

CCEaOK/TfiSEE/     2;i,a2;,A23,A31,a32,A3  3,31,3; 


=  9.3 


MZ  =  X  (S) 

TGC=-  (  FX*VHX  +  FY      'FY  +  EZ 

IF  (TGO.Li.O.)     GC     IC 


)  /  (  VEX  >*2  +  VSY*  '2  t-V3Z**2) 


F0=  (CT*TGC-T. +E2XE  (-CT*TGC)  )/  |( 
ZEKX=  3  X+  V  EX*~GC+  • 
ZEKY=P.  K+  liEX*TGC  +  AIY*FQ 
ZEJiZ=RZ+VEZ*TGC  +  ATZ*  EO-.  5  '1*  (TGC 

Z  EZ  =  A3  1  *ZE  ax  +  A  3  2*Z£*Y  +  A  3  3  *Z  E3  Z 
P  1  =  3.* TGC/ (3.*E1 *-IGO**3) 

ac=-?i  *5 

DEHI=DA?AS2  (ZEY,  -ZF.Z) 

F  2  =  7 . *  AC  * AC  *TGC*  ( 1GO  **3  *■  3  .*  S 1  ) 

F3=  (AC*AC*TGO**5)  ♦  (c.3.*32) 

PC=F2*DPEI/F3 

GC    TO     IOC 

CONTINUE 

AC=0. 

p  C  =  0  . 

CCS1INUE 

£5C 
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55 

60 

65 

7C 
75 


3UE3C3 
IHELIC 

COMMON 

G  =  :  . 

:iur  -,:  = 

THTC=0 
FSI0=C 
VC=DEI 

.:  2=t  ... 
•£C  =  u^I 

7  XC  =  V0 

r::=  :  j 
7ZC=-V 


riSS    E?iT2G  (EZIK,a: 

IT     52  A  L  *  €      (i! - H  , 0- Z ) 

lOi    DJSIM  (  16)  ,  EEOUT  {  16)  ,  Zl  1  ( 1    ) 

,•  r»C/    SC,EC,ATXO,     EKO  ,aizo  ,ctx 

16 

MS 


,  c  x ; 


r  OT ,      ■      . 


0  *  C  £1 N  (  I H 1 0 ) 


(?S  10) 


NUHE2ICAI    IN  TEGS  A  TIC N     ( 


•08 ESI     RUi    ;  j  -  SO 


I 


DC 
T=D 
1HI 
ESI 

v  = :: 
PHI 
A  .v.  = 
EH= 
I?  ( 
1*  I 
DSC 
DEC 
CZC 
DEC 
DEC 

ego 

Z  iC 
D  EC 
DEC 

DEC 
EEC 
DEC 

-  v  p  ; 

dec: 

DEC! 

eec< 
i?  ( 

IE  ( 

DC 
IF  I 

if! 

DE1 

DE2 

GC" 

USE 

CE2 

USE 

GC 

USE 

CZ2 

USE 

GC 

US? 

USE 

DEI 


75     .11=1,4 
SIN  (1) 

=  p£,IN  I     : 

Mil) 


IS? 
I  A  C 
UT  ( 

UT( 

UT( 
UT  ( 

UT 


C.  G 

a: 

i)  = 

5! 


(7) 
UT  t  6  ] 
□T{9) 
DT(10) 
UT  (11 
12 
13 

14 

15 
16 
IN 

C. 


la  i  . 
I.  J 

1 . 

:  (A.I 
'Ail* 

-Q* 

'  '.  *  D 

■  V  *  D 
■■  A  T  X 

■  &  r  : 

=  AT 


!f'»! 


)  =1 


!=C  SIN  (1  5) 
=C E I S  (  it)] 

5  ItHI)  -G*CCO£  ITHT))  /V 

!r::ij  /{v*ccos  r...  : ; 

IIHT) 

1KT)   *~C23  (?SI) 
IHT)  *DSIN  (PS 

..:  i  -ctx*?) 

XE <-CTY*T ) 
£XP(-CTZ*S) 


75 

ai. 

* 1 . 
21. 


i=6 

TO 

1  =  3 

Hi 

TO 
1  =  D 


Eg. 
-  ^ 

EC. 
=  D; 
=  D. 
E2 
75 
EG 
=  D 
5  E 
75 


rQ3 

=  e« 

=  0. 

=  J . 
GI. 
T.  J 

-;;G 

2) 

4) 

i:<'( 

CUT 


(1. 


;  _ :_  i  c r:  i  n  5  ;  -  ( ? c  - 1 .- )  /ts  e  i  k 


To     = 


8  A  I 

GC  TC  60 
GC  TC  65 
GC    TC    7  0 

I)  »CT 


c  i  =  u . 
TO 
1=3 
1=< 

N(i: 


UT (I)  *  2- *CT 
E2 (I)  + IS51 
1*.25 

i)  *;.*ci 

I     +  CSE1 


Dlfl)  *C1 

2  (I)  +USE1)/b. 

D  E  1  ( I )  +  C  S  E 1 


IF(TSPIN.IE.O„)     DEIS  |15)=?C 
IE  ITACC.  LZ.O  .  )     CEIN  (  16)  =AC 

TiiT=D2IN  (2) 
P£I=DEIN  <3) 

v=czi:i  i-o 

VX=V*QCOE  (THT) *DCCS  (ESI) 
V  1=  7  *  D  CC  S  |T  HT )  *  CS  IK  (ESI) 
VZ=-V*DSI5  (THT) 
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B  < 1 ] =D  EI S  j     1 

-  (  JC  +  VXO*DT) 

5  (2}  =  QEJ 

-  1  Y  C  +  V  Y  0 

B  {2)  -- 

-  (  Z  C  i-  V ZO  *C 

c  ('»)  =  ,'  i-  .  xo 

E  ( 5 )  =  V  {  -  1  \ 

5  (6)  =V2- 

B  (6)  =0. 
2  |9)=0. 

RcTU]  N 

ESC 
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3  0 


100 
1  5  C 


i 
.  i     ... 


'  { A  ,  X  ,  9  ,  S 

I    5  J    1  =  1  , i 
XX  (I)  =  AX  |I)-e  (J) 
CCKTINU2 

iX  IRAPGLAIE 

CAL 


EX2HAPGLA1E     :CVASIAfiC2 

CALL    VMOLFF (?,A1,S,9,9,9  ,9,? AT, 9, 12) 

CALL     /MDLFF (A  ,5*1,9,3,9 ,9, 9, A  EAT,  ;,I3) 

DC     15  J    1=1,9 

CC     100     0=1,3 

?  ?  1 1  .  J  )  =  A  £  A  T  1 1  .01  +  C  1 1  .  J ) 


??  [I, J)  =  A£AT  (I  ,0 
CONTINUE 

cc :i xi:j  ue 

E  C 8  a    H    >.:,z    .i  :    X  A 1  s  I C  ES 

X1  =  XX  (  1) 

■  ( 2 ) 
x.;  =  xx;5) 
:c  u  =  x  x  ;  -•  i 

X5  =  XX    5) 
X  6 = X  X  ( 6  i 

D    =DS0 3T  (X1**2+X2**2+X3*     . 

.... ■ .-/:  (11**2+X2**2) 
E2=X1»XU *X2*XS *X3*X6 
E1=XU+X6 
E2=X5+ X6 
E3=XU-X5 
EU=D1*D1+2.*D*C 
C3=X1*X3 
D<J=X2*X3 
D  5  =  ( X 1  *  X 
C6=X2*E3 
D7=X1*E3 


*  (2.  *E*C*E1*E1*E  1-  (X1*X1*EH-X2*X2*£2)  *  (3  .  *D  1*D  1  +  D*D) 
»  (2.  *  C  *  D  *  E  1  *  C 1*S  2- ( X  1*X1*E1+X2*X2*E2)  *  (j.*D1*=0  1+D*C) 
1*E1+X2*X2*E2)*  (D*D-X3*X3) 


1  m  *X  1*21 


-o*  c  *c  1  *i 
-d*d*di*di+x2*x2*eiO 


DC    250    1=1,6 

CC    200    0=1,9 
■  1 1,  J)  =0. 


20  0 

CCSTISUE 

25C 

ccxir.i  us 

H  (1,1)  =X 

a  (1,2)  =x 

H  (1,3    =- 

a  <  2 ,  i )  =  d 

K  (2,2)  =D 

K (2,3)  =D 

H  (2,4)  =X 

H  (2,3    =  X 

H  (  u,  6)  =D 

a  |3,1}=- 

H  (3,2)  =X 

H  (<.,1)  =C 

H  (4,2)  =£ 

a  (4,3)  =( 

a  (u,5)  =  | 

H  (5,  1)   =  X 

H  (5,2)  =X 

1*X3/  |E1*C*D) 
2*X3/  (C1*E*C) 

C  1/  (D*C) 
2  /  I  i D  *  *  5 


3)  ) 

J)  ) 


2/  ((0**5)  *  (C1 

«/((C**3l*(C1 

5/(    D**5) *D1) 

1  *Sl *X3/  (C*C*D*D1 ) 

2*X2*X2/  (l*C*D*0  1) 

1*X3/  |E*D*C) 

X2/ (d*C1) 

1/  (D  1  *C1) 

fc/  ((E**3)  MCI"'; )  ) 

//  (  (0**3) *  (C1**4J  ) 

X1*X2*J3*E3)/((D**3 

-X1*X2)/  IC*E1*01) 

X  1  *  X  2  )  /  ( G '  D  1  -  D  !  ) 

1/D 


*  |E1**2)  ) 


16  5 


50  0 
35C 


:0  0 
U5C 


5CC 


55C 


500 
65C 


H  (5,  3)  =X3/D 

H(c,1)=(C*0*XU-D2*X1)/(D* 

H<6,2)=(£*D*X5-E2*X2)/(C*D*D 

H  1 6  ,  3 )  =  ,  E  * .  ■■  ■-.  .  ■■■      | 

I  .    •  )     =  A  1  /  J 

H  (6,5)  =  X2/D 
U  (6  ,  S)  =  X3/D 

CO  350  1=1,9 
OC  30 j  J=1,b 
HI  II, J]  =H  |J,I) 

"  J  :; E 

compute   kalman    saih   matrix 

CAII    / ;iu iff  ( ??,::, ,.: ,  ;,o  ,  j  ,i  ,?h:,  ^  ,i-  ) 
CALL    VXUL5F (H ,£HI , 6 , S,6 ,6  ,9 ,H  EET,6,I 5) 

DC    «50    1=1.6 

C  C    4  G  0    J  =  1 ,  o 

i  li, j)  =;:;hi  (i  ,j)  f  s  u  ,0) 

CCKTIMUE 
CCNXIbJ  UE 

INVERT  Y  » A  T S I i 

CALL  LISV1F(X,6,6,XI!>7,0,  *ORK,IER)   ' 

CALL    VflULFF  {PHI,  YIJiV  ,9,5  ,  6,9  ,£,G,9,I6) 

UPDATE    SI ATE     VECICS 

KH(1)=-DA1AS2<X3,E1) 

HH<2)=({X1*X1*X3*XU)+(X2*X2*X3*X5)  +  (E1*E1:!=X3*       I     /  i1     -  ■  '  I 

HK  |  j|  =DA1AN2  (X2  ,  XI) 

HH(aj  =  ({-X1*X2*X4)+|     i  *  X  2   'X5))/(G*D1*£1) 

HH  (5)  =C 

H  S  ( 6 )  =  D  2  /  C 

DO     5  CO    1  =  1,6 

C2  (I)=Z  (I)-HH  (I) 

CONTINUE 

call  vhuiff (G  , : : 

DC    550    1=1,9 

X  (I)=XX  (I)  *-DX  |I) 

[SUE 


,1,9,6,DX,9,I7) 


UFEATE     CCVARIAKCE 

CALL    VHUIFF(G,H,9,6,S,9,6  ,GH,9,I8) 

DC    £50    1=1,9 
CC    6  00    J =1,9 
P  <I,Jj   =DEI  (I 
CCSTINOE 
C 0 S TIN  UE 


i)-GH  |l  ,0) 


CALL    VMOIF? (F,EP,S,9,9,9  ,' 

RF.TURH 
EKE 
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IJiFLIC 
V=£AT* 

;.  =  c  * :  r 

s  1  =  DEX 

E2=C£X 

V  11  =  7/ 

V  1  2  =  7  / 

m  )=  <■ ,' 

I  2  2  --  /  • 
7  2i=V/ 
V33  =  V 

Q  11  =  7  1 
£12=V1 

;  15= ;  1 

C22=V2 
Q23=V2 

C32=VJ 


TIN;    PI!     C  |  C,DT,f  1,F2,i  3,Q  I  1 ,  *  12  ,Q  1  3  ,  J. 

II    SEAI^E     (A-H  ,0-2) 

*2 

Ff-fl) 

F  {  -  2 .  *  A ) 

(C**3j 
(C**2) 


1  *  [1.-S2+2.*A+  1 2 .  / 3 . ;  *  (  A  *  *  3 ) - 2 .  - 

2  *  {  H  2  +  1  .  -     .  •  -   I  +  2 .  *  A  *  S  1  -  2  .  *  A  +  A  *  A ) 
J*  [  1.-52-  2-  *A*  HI) 

'2*  {t    *  •  1  -  ;  -  -  -  2  + 2 .  *  \  ) 
13*  |£2+1 .-2.*£1) 
3*  [  1.-52} 


:1) 


ml- 

EKE 


l.+E1)/(C**2) 
-t  1)/C 
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IOC 


20i 


/* 

/* 


SCEaOOTIX  I    ST  SI  (X,M,  AVG 
IMPLICIT     3  E  .M  J  c      t  A  -  .-  ,  0- 
£  E  JiS  10  S    .((50) 

sue=o. 

DC  ICO  1=1, M 

so2=sa«+x  (i) 

CCSTIN DE 
SVG=5U 2/ 8K 

£  t) .".  i 

£  C  2  20  1=1,3 

I)-.A  vG)  **2 
C  C  :.  :  .  I  E 

3  M  S =0  £  S  U  iJ  2  /  A  li  ] 

EETUfiN 
END 

.5YSIN    OD     * 


16S 
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